Sorting through code from Fiverr programmer

I went on Fiverr, and paid someone to write some sketches for my project. What I got looks good, but functionally isn't what I was hoping for. Short story, I haven't been able to get a response from the person. Now I need to figure out what I am looking at. The base project is shown in the picture. The code for the Mega, Mini, and the ESP-01 are below. My main questions are as follows.

  • The motor/motor controller/AS5600 are supposed to act as a servo, using P.I.D. controllers. With #define Motor1Pin 0, is that defining a single pin for control? How do I alter this to allow for 2 pin (directional) control?
  • It looks like they used arrays for the 'tune' function. I need to be able to directly control the input values to each 'servo'. How can I do that?
  • The ESP-01 connects over a wi-fi network to a PC running Node-Red. Is there a way to send an indication showing the connection is valid and active?

I am no programmer, but I can modify code if I understand what I need to do. I have a ton of questions, and I want to keep going. Also, I will still look at getting another programmer to finish coding all this.
Thanks for any help!

Code for Mega

#include "Vars.h"


void setup() {
  for(int i = 0; i < 6; i++){
    Motors[i].setup();
  }
}

void tune(){
  for(int i = 0; i < 6; i++){
    Motors[i].tune();
  }
}

void loop() {
  for(int i = 0; i < 6; i++){
    Motors[i].loop();
  }
  
  if(Serial.available()>0){//0,5.255
      String data = Serial.readString();
      int device_num = data.charAt(0);
      if(device_num == '0'){
        int comma = data.indexOf(',');
        int period = data.indexOf('.');
        int motor = data.substring(comma, period).toInt();
        int speed = data.substring(period+1).toInt(); 
        if(motor < 6){
          Motors[motor].setSpeed(speed);
        }else{
          tune();
        }
      }
  }
}

Vars.h

#include <Wire.h> //This is for i2C
#include <PID_v1.h>
#include <PID_AutoTune_v0.h>
#define TCAADDR 0x70
 
class encoder {
  //Magnetic sensor things
  int magnetStatus = 0; //value of the status register (MD, ML, MH)

  int lowbyte; //raw angle 7:0
  word highbyte; //raw angle 7:0 and 11:8
  int rawAngle; //final raw angle 
  float degAngle; //raw angle in degrees (360/4096 * [value between 0-4095])
  
  int quadrantNumber, previousquadrantNumber; //quadrant IDs
  unsigned long prevMillis = 0;
  float prevTurns = 0; //number of turns
  float numberofTurns = 0; //number of turns
  float correctedAngle = 0; //tared angle - based on the startup value
  float startAngle = 0; //starting angle
  float totalAngle = 0; //total absolute angular displacement
  float previoustotalAngle = 0; //for the display printing

  public:
    encoder(){
    }

    void ReadRawAngle(){ 
      //7:0 - bits
      Wire.beginTransmission(0x36); //connect to the sensor
      Wire.write(0x0D); //figure 21 - register map: Raw angle (7:0)
      Wire.endTransmission(); //end transmission
      Wire.requestFrom(0x36, 1); //request from the sensor
      
      while(Wire.available() == 0); //wait until it becomes available 
      lowbyte = Wire.read(); //Reading the data after the request
     
      //11:8 - 4 bits
      Wire.beginTransmission(0x36);
      Wire.write(0x0C); //figure 21 - register map: Raw angle (11:8)
      Wire.endTransmission();
      Wire.requestFrom(0x36, 1);
      
      while(Wire.available() == 0);  
      highbyte = Wire.read();
      
      //4 bits have to be shifted to its proper place as we want to build a 12-bit number
      highbyte = highbyte << 8; //shifting to left
      //What is happening here is the following: The variable is being shifted by 8 bits to the left:
      //Initial value: 00000000|00001111 (word = 16 bits or 2 bytes)
      //Left shifting by eight bits: 00001111|00000000 so, the high byte is filled in
      
      //Finally, we combine (bitwise OR) the two numbers:
      //High: 00001111|00000000
      //Low:  00000000|00001111
      //      -----------------
      //H|L:  00001111|00001111
      rawAngle = highbyte | lowbyte; //int is 16 bits (as well as the word)
    
      //We need to calculate the angle:
      //12 bit -> 4096 different levels: 360° is divided into 4096 equal parts:
      //360/4096 = 0.087890625
      //Multiply the output of the encoder with 0.087890625
      degAngle = rawAngle * 0.087890625; 
      
      //Serial.print("Deg angle: ");
      //Serial.println(degAngle, 2); //absolute position of the encoder within the 0-360 circle
      
    }
    
    void correctAngle(){
      //recalculate angle
      correctedAngle = degAngle - startAngle; //this tares the position
    
      if(correctedAngle < 0) //if the calculated angle is negative, we need to "normalize" it
      {
      correctedAngle = correctedAngle + 360; //correction for negative numbers (i.e. -15 becomes +345)
      }
      else
      {
        //do nothing
      }
      //Serial.print("Corrected angle: ");
      //Serial.println(correctedAngle, 2); //print the corrected/tared angle  
    }
    
    void checkQuadrant(){
      /*
      //Quadrants:
      4  |  1
      ---|---
      3  |  2
      */
    
      //Quadrant 1
      if(correctedAngle >= 0 && correctedAngle <=90)
      {
        quadrantNumber = 1;
      }
    
      //Quadrant 2
      if(correctedAngle > 90 && correctedAngle <=180)
      {
        quadrantNumber = 2;
      }
    
      //Quadrant 3
      if(correctedAngle > 180 && correctedAngle <=270)
      {
        quadrantNumber = 3;
      }
    
      //Quadrant 4
      if(correctedAngle > 270 && correctedAngle <360)
      {
        quadrantNumber = 4;
      }
      //Serial.print("Quadrant: ");
      //Serial.println(quadrantNumber); //print our position "quadrant-wise"
    
      if(quadrantNumber != previousquadrantNumber) //if we changed quadrant
      {
        if(quadrantNumber == 1 && previousquadrantNumber == 4)
        {
          numberofTurns++; // 4 --> 1 transition: CW rotation
        }
    
        if(quadrantNumber == 4 && previousquadrantNumber == 1)
        {
          numberofTurns--; // 1 --> 4 transition: CCW rotation
        }
        //this could be done between every quadrants so one can count every 1/4th of transition
    
        previousquadrantNumber = quadrantNumber;  //update to the current quadrant
      
      }  
      //Serial.print("Turns: ");
      //Serial.println(numberofTurns,0); //number of turns in absolute terms (can be negative which indicates CCW turns)  
    
      //after we have the corrected angle and the turns, we can calculate the total absolute position
      totalAngle = (numberofTurns*360) + correctedAngle; //number of turns (+/-) plus the actual angle within the 0-360 range
      //Serial.print("Total angle: ");
      //Serial.println(totalAngle, 2); //absolute position of the motor expressed in degree angles, 2 digits
    }
    
    void checkMagnetPresence(){  
      //This function runs in the setup() and it locks the MCU until the magnet is not positioned properly
    
      while((magnetStatus & 32) != 32) //while the magnet is not adjusted to the proper distance - 32: MD = 1
      {
        magnetStatus = 0; //reset reading
    
        Wire.beginTransmission(0x36); //connect to the sensor
        Wire.write(0x0B); //figure 21 - register map: Status: MD ML MH
        Wire.endTransmission(); //end transmission
        Wire.requestFrom(0x36, 1); //request from the sensor
    
        while(Wire.available() == 0); //wait until it becomes available 
        magnetStatus = Wire.read(); //Reading the data after the request
    
        //Serial.print("Magnet status: ");
        //Serial.println(magnetStatus, BIN); //print it in binary so you can compare it to the table (fig 21)      
      }      
      
      //Status register output: 0 0 MD ML MH 0 0 0  
      //MH: Too strong magnet - 100111 - DEC: 39 
      //ML: Too weak magnet - 10111 - DEC: 23     
      //MD: OK magnet - 110111 - DEC: 55
    
      //Serial.println("Magnet found!");
      //delay(1000);  
    }
    
    void setup(){
      checkMagnetPresence(); //check the magnet (blocks until magnet is found)
      ReadRawAngle(); //make a reading so the degAngle gets updated
      startAngle = degAngle; //update startAngle with degAngle - for taring
    }

    void loop(){
      ReadRawAngle(); //ask the value from the sensor
      correctAngle(); //tare the value
      checkQuadrant();
    }

    int getSpeed(){
      loop();
      int speed = (numberofTurns - prevTurns)/(millis()-prevMillis);
      prevMillis = millis();
      return speed;
    }

};

class MotorPID {
  
  double Setpoint;
  double Input;
  double Output;
  double Kp;
  double Ki;
  double Kd;
  int num;
  int pin;
  int maxspeed;
  PID myPID;
  
  PID_ATune aTune;
  encoder myEncoder;
  
  public:
    MotorPID(int n, int p) : myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT), aTune(&Input, &Output), myEncoder() {
      num = n;
      pin = p;
    }

    void setup(){
      pinMode(pin, INPUT);
      myPID.SetTunings(Kp,Ki,Kd);
      myPID.SetSampleTime(1);
      myPID.SetOutputLimits(-255, 255);
      myPID.SetMode(1);
      tcaselect(num);
      myEncoder.setup();
    }

    void loop(){
      tcaselect(num);
      myEncoder.loop();
      myPID.Compute();
      analogWrite(pin, Output);
    }

    void tune(){
      analogWrite(pin, 255);
      delay(100);
      maxspeed = myEncoder.getSpeed();
      analogWrite(pin, 0);
      aTune.SetNoiseBand(500);
      aTune.SetOutputStep(1);
      aTune.SetLookbackSec((int)20);
      if(aTune.Runtime()){
          Kp = aTune.GetKp();
         Ki = aTune.GetKi();
         Kd = aTune.GetKd();
         myPID.SetTunings(Kp,Ki,Kd);
         myPID.SetMode(1);
        }
  
    }

    void setSpeed(int speed){
      tcaselect(num);
      int temp;
      map(temp,-255, 255,-maxspeed,maxspeed);
      Input = speed - temp;
      loop();
    }

    int getSpeed(){
      return Output;
    }

    void tcaselect(uint8_t i) {
        if (i > 7) return;
       
        Wire.beginTransmission(TCAADDR);
        Wire.write(1 << i);
        Wire.endTransmission();  
    }
    
};

MotorPID Motors[] = { MotorPID(0,Motor1Pin),MotorPID(1,Motor2Pin),MotorPID(2,Motor3Pin),MotorPID(3,Motor4Pin),MotorPID(4,Motor5Pin),MotorPID(5,Motor6Pin) };

Code for Mini

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// you can also call it with a different address you want
//Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);

#define SERVOMIN  150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX  600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN  600 // This is the rounded 'minimum' microsecond length
// based on the minimum pulse of 150
#define USMAX  2400 // This is the rounded 'maximum' microsecond length 
// based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

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

  pwm.begin();
  
  pwm.setOscillatorFrequency(27000000);
  pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates

  delay(10);
}

void driveServo(int servo_num, int degrees){
  int pulselength = map(degrees, 0, 180, SERVOMIN, SERVOMAX);
  pwm.setPWM(servo_num, 0, pulselength);
}

void loop() {
    if(Serial.available()>0){//1,16.120
      String data = Serial.readString();
      int device_num = data.charAt(0);
      if(device_num == '1'){
        int comma = data.indexOf(',');
        int period = data.indexOf('.');
        int motor = data.substring(comma, period).toInt();
        int deg = data.substring(period+1).toInt(); 
      }
  }
}

Code for ESP - SSID and PW changed for security.

#include <ESP8266WiFi.h>
#include <PubSubClient.h>
#include "Arduino.h"

const char* ssid = "XXXXXXX";
const char* password = "XXXXXXX";
const IPAddress mqttServerIP(192,168,254,14);
const int httpPort = 1883;
const char* deviceID = "ESP8266";

WiFiClient wifiClient;
PubSubClient MQTTClient(wifiClient);
long lastMsgTime = 0;
char msg[64];
char topic[32];
int pulseCount = 0;

/*
 * Code for MQTT reseves
 * 0.{number of motor},{speed}
 * 1.{number of servo},{degree}
 */
void mqttCallback(char* topic, byte* payload, unsigned int length){
  memcpy(msg, payload,length);
  msg[length] = '\0';
  
  String message(msg); 
  int comma = message.indexOf(',');
  int period = message.indexOf('.');
  
  if(message.charAt(0) == '0'){
    int motor_num = message.substring(period+1, comma).toInt();
    String speed = message.substring(comma+1);
    if(motor_num<6){
      Serial.println("0."+String(motor_num)+","+speed);
    }else{
      Serial.println("0."+String(motor_num));
    }
  }else if(message.charAt(0) == '1'){
    String servo_num = message.substring(period+1, comma);
    String degree = message.substring(comma+1);
    Serial.println("1."+servo_num+","+degree);
  }

  /*
  Serial.print("Message recived in topic [");
  Serial.print(topic);
  Serial.print("]  ");
  Serial.println(msg);
  */

  //Here is where to put stuff to act when somthing is recived
}

void wifiSetup(){
   delay(10);
  // We start by connecting to a WiFi network
  /*
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);
  */

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    //Serial.print(".");
  }
/*
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP()); 
  */
}

void mqttSetup(){
  MQTTClient.setServer(mqttServerIP, 1883);
  MQTTClient.setCallback(mqttCallback);  
}

void setup() {
  Serial.begin(9600);
  wifiSetup();
  mqttSetup();
}

void mqttLoop(){
  while(!MQTTClient.connected()){
    /*
    Serial.print("Attempting to connect to MQTT broker at ");
    Serial.println(mqttServerIP);
*/
    if(MQTTClient.connect(deviceID)){
      //Serial.println("Connected to MQTT broker");
/*
      snprintf(topic,32,"ToHost/%s", deviceID);
      snprintf(msg, 64, "CONNECT", deviceID);
      MQTT.publish(topic,msg);

      snprintf(topic, 32, "ToDevice/%s", deviceID);
      MQTTClient.subscribe(topic);
*/
      MQTTClient.subscribe("test");
      
    }else{
      Serial.print("Connection Failed");
      delay(5000);
    }
  }
  MQTTClient.loop(); 
}

void publish(char* message){
  snprintf(topic, 32, "test", deviceID);
  MQTTClient.publish(topic, message);
}

void loop() {
  mqttLoop();
  /*
  if(Serial.available()>0){
    String data = Serial.readString();
    char temptopic[32];
    data.toCharArray(temptopic, 32);
    publish(temptopic);
  }
  */
}

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the </> icon above the compose window) to make it easier to read and copy for examination

Which libraries are you using for the PID code? I'm not going to go though all the libraries looking for PID_v1.h and PID_AutoTune_v0.h

The servo code for the Mini basically does nothing. It sets up the servo controller board, reads data from serial and parses the serial data, but never actually does anything with the servos.

Is there some specific reason for using a Mega, Mini, and ESP-01?

The "Vars.h" file is using a bunch of undefined pin numbers:

MotorPID Motors[] =
{
  MotorPID(0, Motor1Pin),
  MotorPID(1, Motor2Pin),
  MotorPID(2, Motor3Pin),
  MotorPID(3, Motor4Pin),
  MotorPID(4, Motor5Pin),
  MotorPID(5, Motor6Pin)
};

What kind of H-Bridge motor controller uses only a single pin for speed and direction?!?

It looks like they are trying to send negative numbers to analogWrite():

      pinMode(pin, INPUT); // ??????
      myPID.SetOutputLimits(-255, 255);
...
      myPID.Compute();
      analogWrite(pin, Output);

This function is very 'sus' as the kids say:

    void setSpeed(int speed)
    {
      tcaselect(num);
      int temp;
      map(temp, -255, 255, -maxspeed, maxspeed);
      Input = speed - temp;
      loop();
    }

Very little of this makes sense. The 'temp' variable is not initialized but is used as an argument to 'map()' but the result of 'map()' isn't used so nothing happens. Then the uninitialized 'temp' is subtracted from 'speed' and stored in 'Input'?!?

Little wonder that there is no response from the author of the code

What is this apparatus supposed to do?

Looks animatronic to me. 16 hobby servos in the "head", plus six gear-motors with feedback to act as six high-power servos.

1 Like

The pin assignments are one of the issues I was hoping to fix. The BTS7960 and any other driver I know of, use 2 pins. I think the programmer didn't understand this is positional, not rotation speed.

As Johnwasser determined, it is an animatronic device. Every set (motor, controller, and AS5600) is designed to be given a position just like a servo. I needed more power, for certain sections, than most servos will give.

I'll have to check which libraries when I get home.
As far as using the Mega, Mini, and ESP-01, it seemed like it would be easier to divide the functions between the 2 Arduino's, since this seems like a pretty data heavy setup. I could be wrong. I didn't think the Mega would handle all of this well on it's own.

I was thinking it might be better to run everything on an ESP8266 (or the newer ESP32). Would probably need a second I2C PWM board to drive the motor controllers.

https://github.com/imax9000/Arduino-PID-Library

That library lacks both the files needed for the Vars.h header file. The previous version of the library has PID_v1.h, but neither library has PID_AutoTune_v0.h

#include <PID_v1.h>
#include <PID_AutoTune_v0.h>

The Arduino MEGA alone can produce 48 servo outputs. It might make sense to put a small processor at each of the big motors to receive servo pulses from the MEGA and drive the motor autonomously.

Ok, that is something I can fix easy enough. I'll see about getting the proper library. Thank you!

I would rather keep the number of boards to a minimum if possible. Adding 6 (or more) additional processors seems overly complex. If the Mega becomes a limitation, I had a suggestion to switch to an ESP8266, or ESP32. For now, I would like to work with the current setup since I have already assembled the hardware. I'll see how things progress before I decide on hardware changes. :slight_smile:

Is anything working at the moment? Much of that code looks half-finished at best, and completely non-functional at worst.

If you want any substantial help, you will need to give a complete description of exactly how this is supposed to work, how it is being controlled, the exact format of the control instructions, etc.

As of this moment nothing is working, so I'll give a basic rundown of the whole system.

This is an animatronic setup. The base of the system is the Mega, and Pro Mini. The Mega being used to mainly run the 6 motor controllers, process the position data from the AS5600's, and use PID control to change the motor positions similar to a servo. An I2C multiplexer transfers data from the 6 AS5600's(fixed I2C address) to the Mega. The ESP-01 is connected over a wi-fi network, to a PC running Node-Red and Mosquitto MQTT Broker. At the moment, the Node-Red JSON isn't working the right way either, and I am working people in their forum to get it working. The data that should be coming to the Mega should be in microseconds for the digital servos, and an angle measurement for the motor controller/AS5600 sets.
The Pro Mini is designed to operate the digital servo's via the PWM servo shield, and free up a bit of the load on the Mega.
All of this was supposed to operate on milli timing, and be updating every 50-100 ms.
I hope that is at least a bit clearer. If you want to see the JSON from Node-Red, I can post that too.

Your circuit diagram has a couple of major problems.

First, you are powering the Pro Mini with 5V going to the RAW input, this should go to the VCC input (unless it is a 3.3V board, which would lead to more problems).

Second, you have a single I2C bus connecting the Mega, the Pro Mini, the I2C multiplexer, and the servo PWM board. Presumably the Mega is communicating with the I2C multiplexer and Pro Mini, while the Pro Mini is communicating with the servo PWM board. This will lead to conflicts when the Mega and Pro Mini try to use the I2C bus simultaneously, and also puts the Pro Mini into a situation where it is switching between master and slave modes - not something an arduino handles well.

Presumably the Mega is forwarding the servo commands to the Pro Mini over I2C, since there are no serial connections to the Pro Mini. This does not make much sense, the Mega is already parsing the input data, might as well have it send the command to the servo PWM board, instead of sending the command to the Pro Mini which then sends it over the same I2C bus to the PWM board.

agree.
Each additional microcontrol doubles the problems.
Only use one ESP based board.
Don't use a plain ESP8266 but a Makerfriendly NodeMCU or Wemos.
If necessary, add a second PWM board.

Eliminating the Pro Mini would be easy enough. Makes sense, and would simplify the wiring.

For now I'm going to work with the Mega. Eliminating the Pro Mini is fine and makes this setup cleaner. I'm not sure a second PWM board would work with the motor controller\AS5600 setup. Working with the hardware i have is preferable right now.