Showroom FlyPT - 6DOF Brushless DIY Actuators

Hi , @pmvcda ( need your help )
I try this new set up like your but I have some problems !( With ESp32 position control by internal BLDC sensor ).
What happen :I push button ( EN) on esp32 , the actuator start in the middle to minimum switch fast , stay there like (3 sec) and after goes up fast to maximum limit switch and stay there but don't come back in the middle .
The resistances are still 10 k .
Things who are diferents than yours :
1- Level shifter 3.3v /4.6 v other side ( I don't have 5.0v) and the spec is 5v to 24vdc see pic . Before my drive was pushing 6 v....and now 4.6 v...
2-Travel microswitch minimum to maximum is 500 mm ( I thing I have a little bit more travel than you).
3- Diagram of Halls is diferent too . My set up is : (Phase A-B Hall 1 brown / HW gray , Phase B-C Hall 2 orange /HV green , Phase C-A Hall 3 yellow / HU blue ) .
I will try to learn your code c++ on Esp32
Thanks !
Diagram Kollmorgen H-344-h-0000

Hi,

I'm a bit busy these days.
I will post a new code today.
Please use that one for testing. It's faster and that way we are in he same version.

If the calibration speed is to fast, you can lower it on the defines.
In the version you are using, after the reset, the behaviour should be:

- Led blinks for 5 seconds before starting calibration
- Actuator goes down looking for minimum
- Actuator goes up looking for maximum
- Actuator goes to the middle really fast (position zero) and waits for serial commands

In the new version (11.0.is) the behaviour is:

- Led blinks for 5 seconds before starting calibration
- Actuator goes up looking for maximum
- Actuator goes down looking for minimum
- Actuator stays down at minimum position and waits for serial commands

The new version is faster and uses a different string format.
Give me some minutes to clean it up and I will post it.
It's still WIP, I'm trying to remove the limit switches and make those pins free.
Limits will be identified by load, when we want speed and have no movement.
I can do it on my actuators, but that depends on the construction of the actuator and if it allows us to use that pressure on the limits.


Edit:
. We can make more tests with the new version. I just need you to be on the same version to be sure that's not on the code.

@Flymen ,

Here's the new code:

Code:

// ================================
// DIY 6DOF Motion Sim by FlyPT
// Version: 11.0.is from 27-10-2019
// Internal PID calculation
// ================================
// SimTools2 ESP32 code to control up to 3 actuators
// Actuators use brushless motors controlled by BLDC8015A drivers
// The BLDC8015A receives speed and direction in separated pins
// Position is obtained from internall hall sensors


// ===========================
// Serial output configuration
// ===========================
// Interface Type: Serial
// ComPort: The one used by your ESP32
// BitsPerSec: 921600 - If you want feedback on the serial monitor of the Arduino IDE, you have to change this to 500000 in the defines
// Data Bits: 8
// Parity: None
// Stop Bits: 1
// Bit Range: 15
// Type: Binary
// Interface string format: <255><255><Axis1a><Axis2a><Axis3a>
/// Output Rate: Can be down to 1 ms


// ===============
// PID calculation
// ===============
// PID calculation is performed in the ESP32
// In the PID calculation, we are calculating the speed that we want for the actuator.
// We have a pretended position and a current position of the actuator and speed is calculated depending on those values.
// Speed can be from -255 to 255, where negative speed is actuator going down and positive going up.
// Values beyond this range are cropped.
// Define each parameter PID in the defines, where:
// P is the proportional part. The amount of P depends on  the distance


// =======
// Defines
// =======
#define P 256                   // Proportional
#define I 0                     // Integral
#define D 256                   // Diferential
#define SERIAL_SPEED 921600     // Define to set up the serial speed comunication
#define CALIBRATION_SPEED 50    // Speed used to search for minimum and maximum values (0 to 255) if calibration is to fast or to slow, adjust this value
#define MAXIMUM_BIT_VALUE 32767 // Size of position values received (15 bits, above this, we use it to send other controls/requests besides position)
#define UP HIGH                 // Define to set up direction as HIGH
#define DOWN LOW                // Define to set down direction as LOW
#define PWM_RESOLUTION 8        // 0 to 255
#define PWM_FREQUENCY 1000      // The 8015A accepts up to 1 KHz
#define LIMITS_PIN 0            // Array index of pin to capture position from pot wich defines the limits
#define HALL_1_PIN 1            // Array index of pin used for hall 1 interrupt
#define HALL_2_PIN 2            // Array index of pin used for hall 2 interrupt
#define HALL_3_PIN 3            // Array index of pin used for hall 3 interrupt
#define LAST_ANGLE 4            // Array index of last angle that was detected
#define LAST_SUM 5              // Array index of last sum to know last direction it took
#define DIRECTION_PIN 6         // Array index of Pin to se direction
#define SPEED_PIN 7             // Array index of Pin to set speed with PWM
#define PWM_CHANNEL 8           // Array index of channel used to generate PWM
#define MINIMUM_POSITION 9      // Array index of minimum position
#define MAXIMUM_POSITION 10     // Array index of maximum position
#define CURRENT_POSITION 11     // Array index of current position
#define PRETENDED_POSITION 12   // Array index of pretended position
#define P_PID 13                // Array index of P from PID
#define I_PID 14                // Array index of I from PID
#define D_PID 15                // Array index of D from PID
#define INTEGRATED_ERROR 16     // Array index of sum of differences between pretended and current position
#define LAST_ERROR 17           // Array index of last difference between pretended and current position
#define SPEED 18                // Array index of speed of motor rotation
#define FOUND 19                // Array index of foud, used for in actuator calibration
#define NUMBERACTUATORS 3       // Number of actuators (for one board, please don't change, current code does not support less than 3 actuators. To use less, just disconnect them at hardware level)


// ======================
// Actuators information:
// ======================
// 0  - LIMITS_PIN          - Pin to capture position from pot
// 1  - HALL_1_PIN          - Pin used for hall 1 interrupt
// 2  - HALL_2_PIN          - Pin used for hall 2 interrupt
// 3  - HALL_3_PIN          - Pin used for hall 3 interrupt
// 4  - LAST_ANGLE          - Last angle that was detected
// 5  - LAST_SUM            - Last sum to know last direction it took
// 6  - DIRECTION_PIN       - Pin to se direction
// 7  - SPEED_PIN           - Pin to set speed with PWM
// 8  - PWM_CHANNEL         - Channel used to generate PWM
// 9  - MINIMUM_POSITION    - Minimum position
// 10 - MAXIMUM_POSITION    - Maximum position
// 11 - CURRENT_POSITION    - Current position
// 12 - PRETENDED_POSITION  - Pretended position
// 13 - P_PID               - P from PID
// 14 - I_PID               - I from PID
// 15 - D_PID               - D from PID
// 16 - INTEGRATED_ERROR    - Sum of differences between pretended and current position
// 17 - LAST_ERROR          - Last difference between pretended and current position
// 18 - SPEED               - Speed of motor rotation
// 19 - FOUND               - Used as help for calibraion
static int actuator[3][20] = {
//  00  01  02  03  04  05  06  07  08  09  10  11  12  13  14  15  16  17  18  19
  {  4, 27, 26, 25,  5,  0, 19, 17,  0, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 },  // Actuator 1
  { 36, 35, 34, 39,  5,  0, 18,  5,  1, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 },  // Actuator 2
  { 15, 23, 22, 21,  5,  0, 32, 33,  2, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 }   // Actuator 3
};


// =======================================
// Variables for info received from serial
// =======================================
static int bufferPrevious = 0;      // To hold previous read fom serial command
static int bufferCurrent = 0;       // To hold current read fom serial command
static int bufferCount = 0;         // To hold current position in bufferCommand array
static int bufferCommand[6] = {0};  // To hold received info from serial


// ===========================================================================
// Variables used in PID calculation to avoid repeated definitions in the loop
// ===========================================================================
static int limitValue = 1023;
static int error = 0;
static int p = 0;
static int i = 0;
static int d = 0;


// ========================================================
// Array used to calculate motor position from hall sensors
// ========================================================
// Maps to get increment off position from comparing previous hall sensor state with current state
// In the table, vertical is the new value, horizontal is the old one
// Stores for 0 and 7, althought they should not ocurr, digital read might get them. They are ignored with zero change.
// State sequence going up
// 5=101
// 1=001
// 3=011
// 2=010
// 6=110
// 4=100
// First index for new state, second for old state ([new][old])
static int stepUp[8][8] = {
  {  0,  0,  0,  0,  0,  0,  0,  0},
  {  0,  0, -2, -1,  2,  1,  3,  0},
  {  0,  2,  0,  1, -2,  3, -1,  0},
  {  0,  1, -1,  0,  3,  2, -2,  0},
  {  0, -2,  2,  3,  0, -1,  1,  0},
  {  0, -1,  3, -2,  1,  0,  2,  0},
  {  0,  3,  1,  2, -1, -2,  0,  0},
  {  0,  0,  0,  0,  0,  0,  0,  0}
};
static int stepDown[8][8] = {
  {  0,  0,  0,  0,  0,  0,  0,  0},
  {  0,  0, -2, -1,  2,  1, -3,  0},
  {  0,  2,  0,  1, -2, -3, -1,  0},
  {  0,  1, -1,  0, -3,  2, -2,  0},
  {  0, -2,  2, -3,  0, -1,  1,  0},
  {  0, -1, -3, -2,  1,  0,  2,  0},
  {  0, -3,  1,  2, -1, -2,  0,  0},
  {  0,  0,  0,  0,  0,  0,  0,  0}
};


// ==============
// Initialization
// ==============
void setup()
{
  // Disable watchdog in both processors
  disableCore0WDT();
  disableCore1WDT();

  // Setup serial communication
  Serial.begin(SERIAL_SPEED, SERIAL_8N1);

  // Initialize
  for (int n = 0; n < NUMBERACTUATORS; n++)
  {
    // Initialize pins
    pinMode(actuator[n][LIMITS_PIN], INPUT);
    pinMode(actuator[n][HALL_1_PIN], INPUT);
    pinMode(actuator[n][HALL_2_PIN], INPUT);
    pinMode(actuator[n][HALL_3_PIN], INPUT);
    pinMode(actuator[n][DIRECTION_PIN], OUTPUT);
    pinMode(actuator[n][SPEED_PIN], OUTPUT);

    // Setup PWM
    ledcSetup(actuator[n][PWM_CHANNEL], PWM_FREQUENCY, PWM_RESOLUTION);
    ledcAttachPin(actuator[n][SPEED_PIN], actuator[n][PWM_CHANNEL]);
    ledcWrite(actuator[n][PWM_CHANNEL], 0);
  }

  // Create thread that gets position from hall sensors to run in the core 0
  // It will run in paralell with the main loop that uses core 1 by default
  xTaskCreatePinnedToCore(
    TaskUpdatePositions,  // Task function
    "UpdatePositions",    // Name
    10000,                // Stack size
    NULL,                 // Task input parameter
    1,                    // Priority of the task
    NULL,                 // Task handle
    0);                   // Core

  // Initialize pin to light up onboard led and blink for 5 seconds before starting calibration
  pinMode(2, OUTPUT);
  for (byte n = 5; n > 0; n--)
  {
    delay(500);
    digitalWrite(2, HIGH);
    delay(500);
    digitalWrite(2, LOW);
  }
  digitalWrite(2, LOW);
 
  // Calibrate actuators, searching for minimums and maximums
  calibrate();
 
  // Put actuators in the minimum after calibration (for easy access to the rig)
  for (int n = 0; n < NUMBERACTUATORS; n++) actuator[n][PRETENDED_POSITION] = actuator[n][MINIMUM_POSITION];
}


// =========
// Calibrate
// =========
void calibrate()
{
  // Move all actuators to maximum
  // =============================
  for (int n = 0; n < NUMBERACTUATORS; n++)
  {
    digitalWrite (actuator[n][DIRECTION_PIN], UP);
    ledcWrite(actuator[n][PWM_CHANNEL], CALIBRATION_SPEED);
    actuator[n][FOUND] = false;
  }
  int found = 0;
  while (found < NUMBERACTUATORS)
  {
    for (int n = 0; n < NUMBERACTUATORS; n++)
    {
      // Read limit switch position (we are looking for big value 4095)
      // We read more than one time to be sure (4 times in this case)
      if (!actuator[n][FOUND] && (analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN])) / 4 >= 4095)
      {
        ledcWrite(actuator[n][PWM_CHANNEL], 0);
        actuator[n][CURRENT_POSITION] = 0;
        actuator[n][FOUND] = true;
        found++;
      }
    }
  }
  // Move all actuators to minimum
  // =============================
  for (int n = 0; n < NUMBERACTUATORS; n++)
  {
    digitalWrite (actuator[n][DIRECTION_PIN], DOWN);
    ledcWrite(actuator[n][PWM_CHANNEL], CALIBRATION_SPEED);
    actuator[n][FOUND] = false;
  }
  found = 0;
  while (found < NUMBERACTUATORS)
  {
    for (int n = 0; n < NUMBERACTUATORS; n++)
    {  
      // Read limit switch position (we are looking for zero)
      if (!actuator[n][FOUND] && (analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN])) <= 0)
      {
        ledcWrite(actuator[n][PWM_CHANNEL], 0);
        actuator[n][MAXIMUM_POSITION] = -actuator[n][CURRENT_POSITION] / 2;
        actuator[n][MINIMUM_POSITION] = -actuator[n][MAXIMUM_POSITION];
        actuator[n][CURRENT_POSITION] = actuator[n][MINIMUM_POSITION];
        actuator[n][FOUND] = true;
        found++;

        // Send info to serial monitor for debug (use 500000 in serial speed to work with the Arduino IDE)
        Serial.println();
        Serial.print("Actuator ");
        Serial.print(n);
        Serial.print(":  Minimum=");
        Serial.print(actuator[n][MINIMUM_POSITION]);
        Serial.print("  Maximum=");
        Serial.println(actuator[n][MAXIMUM_POSITION]);
      }
    }
  }
}


// ======================
// Update actuator action
// ======================
// Here we calculate the PID depending of desired vs current position and send respective signal to motor controller
void updateActuatorSpeed(byte n)
{
  // Calculate PID
  // =============
  error = actuator[n][PRETENDED_POSITION] - actuator[n][CURRENT_POSITION];
  actuator[n][INTEGRATED_ERROR] += error;
  p = actuator[n][P_PID] * error;
  i = actuator[n][I_PID] * constrain(actuator[n][INTEGRATED_ERROR], -255, 255);
  d = actuator[n][D_PID] * (error - actuator[n][LAST_ERROR]);
  actuator[n][LAST_ERROR] = error;
  actuator[n][SPEED] = FlyPT_Constrain((p + i + d) >> 8, -255, 255); // 2 4 8 16 32 64 128 256 Divide by 256
 
  // Apply changes to motor controller
  // =================================
  if (actuator[n][SPEED] > 0)
  {
    ledcWrite(actuator[n][PWM_CHANNEL], actuator[n][SPEED]);
    digitalWrite (actuator[n][DIRECTION_PIN], UP);
  }
  else
  {
    ledcWrite(actuator[n][PWM_CHANNEL], -actuator[n][SPEED]);
    digitalWrite (actuator[n][DIRECTION_PIN], DOWN);
  }
 
  // Check limits
  // ============
  // Adjust position if minimum or maximum switch is activated in case of a drift in position calculation
  limitValue = (analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN])) >> 2;
  // If they are actuated, recalibrate position
  if (limitValue >= 4095) actuator[n][CURRENT_POSITION] = actuator[n][MAXIMUM_POSITION]; // We are at maximum position
  else if (limitValue <= 0) actuator[n][CURRENT_POSITION] = actuator[n][MINIMUM_POSITION]; // We are at minimum position
}


// ======================================================
// Replacement for constrain function (seems to be buggy)
// ======================================================
int FlyPT_Constrain(int value, int low, int high)
{
  return (value < low) ? low : (value > high) ? high : value;
}


// ====================================
// Update current position of actuators
// ====================================
void updateCurrentPosition(byte n)
{
  int angle = digitalRead(actuator[n][HALL_1_PIN]) * 4 + digitalRead(actuator[n][HALL_2_PIN]) * 2 + digitalRead(actuator[n][HALL_3_PIN]);
  if (actuator[n][LAST_SUM] > 0) actuator[n][LAST_SUM] = stepUp[angle][actuator[n][LAST_ANGLE]];
  else actuator[n][LAST_SUM] = stepDown[angle][actuator[n][LAST_ANGLE]];
  actuator[n][LAST_ANGLE] = angle;
  actuator[n][CURRENT_POSITION] += actuator[n][LAST_SUM];
}


// ===========================
// Main loop running on core 1
// ===========================
// Here we get the simtools commands and make the actuator move
void loop()
{
  if (Serial.available())
  {
    bufferPrevious = bufferCurrent; // Store previous byte
    bufferCurrent = Serial.read(); // Get the new byte
    bufferCommand[bufferCount] = bufferCurrent; // Put the new byte in the array
    bufferCount++; // Change to next position in the array
    if(bufferCurrent==255 && bufferPrevious==255) bufferCount = 0; // Two 255 in sequence are the start of the position info
    if(bufferCount==6) // Having 6 bytes means we have the 3 positions and that we can update the pretended position
    {
      actuator[0][PRETENDED_POSITION] = map((bufferCommand[0] * 256) + bufferCommand[1], 0, MAXIMUM_BIT_VALUE, actuator[0][MINIMUM_POSITION], actuator[0][MAXIMUM_POSITION]);
      actuator[1][PRETENDED_POSITION] = map((bufferCommand[2] * 256) + bufferCommand[3], 0, MAXIMUM_BIT_VALUE, actuator[1][MINIMUM_POSITION], actuator[1][MAXIMUM_POSITION]);
      actuator[2][PRETENDED_POSITION] = map((bufferCommand[4] * 256) + bufferCommand[5], 0, MAXIMUM_BIT_VALUE, actuator[2][MINIMUM_POSITION], actuator[2][MAXIMUM_POSITION]);
      bufferCount=0;
    }
  }

  // Update orders sent to motor driver
  updateActuatorSpeed(0);
  updateActuatorSpeed(1);
  updateActuatorSpeed(2);
}


// ==============================================
// Update positions thread loop running in core 0
// ==============================================
void TaskUpdatePositions(void * parameter)
{
  while (true)
  {
      updateCurrentPosition(0);
      updateCurrentPosition(1);
      updateCurrentPosition(2);
  }
}

I will update the first posts with this soon

Edit:
In the version designation, i means internal PID calculation and s means limit switches
Serial should be:
BitsPerSec: 921600 - If you want feedback on the serial monitor of the Arduino IDE, you have to change this to 500000 in the defines
Data Bits: 8
Parity: None
Stop Bits: 1
Bit Range: 15
Type: Binary
Interface string format: <255><255><Axis1a><Axis2a><Axis3a>

@Flymen ,

The diagram is the same, but colours change:

Code:

Mine   ------------------------   Yours
Red       5V      5 to 24V        Blue
Black     GND                     Green
Blue      H1         CA           Yellow
Green     H2         BC           Orange
White     H3         AB           Brown

Mine is what I have in my motors and on the diagram

Hi ,
I'm lost…
I have this feedback on the serial monitor of the Arduino IDE when the actuator goes down and up with version 11.0 !!
*Jun 8 2016 ???

In the code you have 500000 speed and in the serial monitor 115200.
Try using the same value

What you see there is the Esp32 booting, and the date is related to that.

With the same value !!


Question: I saw it on your code page
- Speed can be from -255 to 255, where negative speed is actuator going down and positive going up.
Your set up is like number 1 RV or 2 AVI :
Speed regulation choice(RV ; AVI)

1. Setup speed by potentiometer (RV).The dipswitch SW2 must be ON status to enable this function. CW rotate the potentiometer will increase speed. CCW- speed down.

2. Setup speed by analog input (AVI). The dipswitch SW2 must be OFF status to enable this function.。AVI terminal accept 0～5V voltage or PWM signal from controller。AVI terminal with input resistance of 100K，current consumption≤5mA。

Because my set up is like number 2 AVI and F/R terminal is applied to shift motor rotate direction, common positive terminal is＋5V.
Thanks again !( take your time body…. montreal 10hpm and Portugal+4h = 2h AM) .

The -255 to 255, is what is calculated.
When I send it out, it's through 2 pins, one for direction and the other with pwm for speed
If speed less than zero, I send low to the direction pin and high if >0 (or the opposite, I have to see the code)
The pwm is generated from the absolute value of the speed.

The setup I use is AVI, so we are in the same setup.

I'm going to look at you schematic to see how you have done it. Give me some minutes.

In your schematic, I think you have H1 and H2 swapped. This makes counting in reverse.
But I'm not 100% sure.

Try to change serial speed to 115200 in the code and the serial monitor of the Arduino IDE
Why?
Because at startup, I think that's the speed you get until you change it.
All those symbols you see are the boot message received at 115200.
After that and after the calibration it should show the minimum and maximum travel of each actuator at the serial speed you specify in the code.
So keep it at 115200 to see all the messages.

How many actuators do you have connected to the board?
Right now, to see the result, you need at least 3
If you are testing just with one, I'm not sure, I need to see it better, but I think you can just change the amount of actuators in the code to that number.

If your schematic was working before with the arduino to specify speed and direction, then it should also work with the ESP32 and your schematic.

How many actuators do you have connected to the board?
Right now, to see the result, you need at least 3
If you are testing just with one, I'm not sure, I need to see it better, but I think you can just change the amount of actuators in the code to that number.
Anwser : -yes , I test only one ( actuator no 2) on output 2 ( actuator 2 on electronic board ).

It better with 3 actuator at same time ?
Thanks again ![/QUOTE]​

Hi,
With serial speed and monitor at 115200 ,I can't see anything ( still date 2016 ….) ! Now I'm in actuator no 1 on the Board
I think you can just change the amount of actuators in the code to that number. need precision ..
#define NUMBERACTUATORS 3 for 1 // Number of actuators (for one board, please don't change, current code does not support less than 3 actuators. To use less, just disconnect them at hardware level)

Hi,
Change this:

Code:

#define NUMBERACTUATORS 3       // Number of actuators (for one board, please don't change, current code does not support less than 3 actuators. To use less, just disconnect them at hardware level)

3 to 1 and connect one actuator. Try the 3 ports, I don't know wich one you defined as the number one

Ignore what I write there about the number of actuators

The problem is that the code waits for the 3 actuators to complete calibration.
To change that , just specify the number of actuators to be 1.
But you have to connect the actuator to the number one!

Good news ! is better !


Now I can see something like that !!
Actuator 0: Minimum=0 Maximum=0

And the actuator board is number 3 who working ….. but he goes down, wait 3 sec after goes up and wait at 2 inch below the maximum microswitch …. it 's like the reverse than you finaly ..
I will try something others… and I will send you a video

Did you swap the hall sensors on the controller like I said?
H1 with H3 to reverse the order

Wait, I think it's H1 and H2.... Damned, I saw the drawing, now looking at the post I have doubts

It's:

H1 <-------> H3

Yes, I did that too … swap H1 and H2 but no …. don't work . I will back up and try swap H1 and H3

finaly no !!!
I have nothing on my monitor serie . @pmvcda
Maybe I need more than 4.6 volts on the sensors … I will see it !
Thanks again !

More test …
This video show the sequence electric of Hall sensor output 1 AB-2 BC-3CA when the actuator goes down ( C.C.W ) .
Fluke Yellow no 1 , Fluke Black no2 , and gray no3.
Video:


It like the table : electrical degrees relative ( see pic.)
Q- Your table have the same timing or phasing ?

The sequence is the same, but one is CW and the other CCW (they are always the same)
This is my sequence compared to yours:

Maybe try swapping H1 and H3 again.
But even exchanged they should show something.
That's what I find strange. I don't see any problem on the code.
Could it be on the limit switches?
Are you sure they are correctly connected?

Please test this code:

Code:

SPECIAL TEST VERSION FOR 1 ACTUATOR (THE FIRST ONE) !!!


// =======
// Defines
// =======
#define P 256                   // Proportional
#define I 0                     // Integral
#define D 256                   // Diferential
#define SERIAL_SPEED 115200     // Define to set up the serial speed comunication
#define CALIBRATION_SPEED 50    // Speed used to search for minimum and maximum values (0 to 255) if calibration is to fast or to slow, adjust this value
#define MAXIMUM_BIT_VALUE 32767 // Size of position values received (15 bits, above this, we use it to send other controls/requests besides position)
#define UP HIGH                 // Define to set up direction as HIGH
#define DOWN LOW                // Define to set down direction as LOW
#define PWM_RESOLUTION 8        // 0 to 255
#define PWM_FREQUENCY 1000      // The 8015A accepts up to 1 KHz
#define LIMITS_PIN 0            // Array index of pin to capture position from pot wich defines the limits
#define HALL_1_PIN 1            // Array index of pin used for hall 1 interrupt
#define HALL_2_PIN 2            // Array index of pin used for hall 2 interrupt
#define HALL_3_PIN 3            // Array index of pin used for hall 3 interrupt
#define LAST_ANGLE 4            // Array index of last angle that was detected
#define LAST_SUM 5              // Array index of last sum to know last direction it took
#define DIRECTION_PIN 6         // Array index of Pin to se direction
#define SPEED_PIN 7             // Array index of Pin to set speed with PWM
#define PWM_CHANNEL 8           // Array index of channel used to generate PWM
#define MINIMUM_POSITION 9      // Array index of minimum position
#define MAXIMUM_POSITION 10     // Array index of maximum position
#define CURRENT_POSITION 11     // Array index of current position
#define PRETENDED_POSITION 12   // Array index of pretended position
#define P_PID 13                // Array index of P from PID
#define I_PID 14                // Array index of I from PID
#define D_PID 15                // Array index of D from PID
#define INTEGRATED_ERROR 16     // Array index of sum of differences between pretended and current position
#define LAST_ERROR 17           // Array index of last difference between pretended and current position
#define SPEED 18                // Array index of speed of motor rotation
#define FOUND 19                // Array index of foud, used for in actuator calibration
#define NUMBERACTUATORS 1       // Number of actuators (for one board, please don't change, current code does not support less than 3 actuators. To use less, just disconnect them at hardware level)


// ======================
// Actuators information:
// ======================
// 0  - LIMITS_PIN          - Pin to capture position from pot
// 1  - HALL_1_PIN          - Pin used for hall 1 interrupt
// 2  - HALL_2_PIN          - Pin used for hall 2 interrupt
// 3  - HALL_3_PIN          - Pin used for hall 3 interrupt
// 4  - LAST_ANGLE          - Last angle that was detected
// 5  - LAST_SUM            - Last sum to know last direction it took
// 6  - DIRECTION_PIN       - Pin to se direction
// 7  - SPEED_PIN           - Pin to set speed with PWM
// 8  - PWM_CHANNEL         - Channel used to generate PWM
// 9  - MINIMUM_POSITION    - Minimum position
// 10 - MAXIMUM_POSITION    - Maximum position
// 11 - CURRENT_POSITION    - Current position
// 12 - PRETENDED_POSITION  - Pretended position
// 13 - P_PID               - P from PID
// 14 - I_PID               - I from PID
// 15 - D_PID               - D from PID
// 16 - INTEGRATED_ERROR    - Sum of differences between pretended and current position
// 17 - LAST_ERROR          - Last difference between pretended and current position
// 18 - SPEED               - Speed of motor rotation
// 19 - FOUND               - Used as help for calibraion
static int actuator[3][20] = {
//  00  01  02  03  04  05  06  07  08  09  10  11  12  13  14  15  16  17  18  19
  {  4, 27, 26, 25,  5,  0, 32, 33,  0, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 },  // Actuator 1
  { 36, 35, 34, 39,  5,  0, 18,  5,  1, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 },  // Actuator 2
  { 15, 23, 22, 21,  5,  0, 19, 17,  2, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 }   // Actuator 3
};


// =======================================
// Variables for info received from serial
// =======================================
static int bufferPrevious = 0;      // To hold previous read fom serial command
static int bufferCurrent = 0;       // To hold current read fom serial command
static int bufferCount = 0;         // To hold current position in bufferCommand array
static int bufferCommand[6] = {0};  // To hold received info from serial


// ===========================================================================
// Variables used in PID calculation to avoid repeated definitions in the loop
// ===========================================================================
static int limitValue = 1023;
static int error = 0;
static int p = 0;
static int i = 0;
static int d = 0;


// ========================================================
// Array used to calculate motor position from hall sensors
// ========================================================
// Maps to get increment off position from comparing previous hall sensor state with current state
// In the table, vertical is the new value, horizontal is the old one
// Stores for 0 and 7, althought they should not ocurr, digital read might get them. They are ignored with zero change.
// State sequence going up
// 5=101
// 1=001
// 3=011
// 2=010
// 6=110
// 4=100
// First index for new state, second for old state ([new][old])
static int stepUp[8][8] = {
  {  0,  0,  0,  0,  0,  0,  0,  0},
  {  0,  0, -2, -1,  2,  1,  3,  0},
  {  0,  2,  0,  1, -2,  3, -1,  0},
  {  0,  1, -1,  0,  3,  2, -2,  0},
  {  0, -2,  2,  3,  0, -1,  1,  0},
  {  0, -1,  3, -2,  1,  0,  2,  0},
  {  0,  3,  1,  2, -1, -2,  0,  0},
  {  0,  0,  0,  0,  0,  0,  0,  0}
};
static int stepDown[8][8] = {
  {  0,  0,  0,  0,  0,  0,  0,  0},
  {  0,  0, -2, -1,  2,  1, -3,  0},
  {  0,  2,  0,  1, -2, -3, -1,  0},
  {  0,  1, -1,  0, -3,  2, -2,  0},
  {  0, -2,  2, -3,  0, -1,  1,  0},
  {  0, -1, -3, -2,  1,  0,  2,  0},
  {  0, -3,  1,  2, -1, -2,  0,  0},
  {  0,  0,  0,  0,  0,  0,  0,  0}
};


// ==============
// Initialization
// ==============
void setup()
{
  // Disable watchdog in both processors
  disableCore0WDT();
  disableCore1WDT();

  // Setup serial communication
  Serial.begin(SERIAL_SPEED, SERIAL_8N1);

  // Initialize
  for (int n = 0; n < NUMBERACTUATORS; n++)
  {
    // Initialize pins
    pinMode(actuator[n][LIMITS_PIN], INPUT);
    pinMode(actuator[n][HALL_1_PIN], INPUT);
    pinMode(actuator[n][HALL_2_PIN], INPUT);
    pinMode(actuator[n][HALL_3_PIN], INPUT);
    pinMode(actuator[n][DIRECTION_PIN], OUTPUT);
    pinMode(actuator[n][SPEED_PIN], OUTPUT);

    // Setup PWM
    ledcSetup(actuator[n][PWM_CHANNEL], PWM_FREQUENCY, PWM_RESOLUTION);
    ledcAttachPin(actuator[n][SPEED_PIN], actuator[n][PWM_CHANNEL]);
    ledcWrite(actuator[n][PWM_CHANNEL], 0);
  }

  // Create thread that gets position from hall sensors to run in the core 0
  // It will run in paralell with the main loop that uses core 1 by default
  xTaskCreatePinnedToCore(
    TaskUpdatePositions,  // Task function
    "UpdatePositions",    // Name
    10000,                // Stack size
    NULL,                 // Task input parameter
    1,                    // Priority of the task
    NULL,                 // Task handle
    0);                   // Core

  // Initialize pin to light up onboard led and blink for 5 seconds before starting calibration
  pinMode(2, OUTPUT);
  for (byte n = 5; n > 0; n--)
  {
    delay(500);
    digitalWrite(2, HIGH);
    delay(500);
    digitalWrite(2, LOW);
  }
  digitalWrite(2, LOW);
 
  // Calibrate actuators, searching for minimums and maximums
  calibrate();
 
  // Put actuators in the minimum after calibration (for easy access to the rig)
  for (int n = 0; n < NUMBERACTUATORS; n++) actuator[n][PRETENDED_POSITION] = actuator[n][MINIMUM_POSITION];
}


// =========
// Calibrate
// =========
void calibrate()
{
  // Move all actuators to maximum
  // =============================
  for (int n = 0; n < NUMBERACTUATORS; n++)
  {
    digitalWrite (actuator[n][DIRECTION_PIN], UP);
    ledcWrite(actuator[n][PWM_CHANNEL], CALIBRATION_SPEED);
    actuator[n][FOUND] = false;
  }
  int found = 0;
  while (found < NUMBERACTUATORS)
  {
    for (int n = 0; n < NUMBERACTUATORS; n++)
    {

        Serial.print("n);
        Serial.print(" SEARCH MAX! Current position= ");
        Serial.println(actuator[n][CURRENT_POSITION]

      // Read limit switch position (we are looking for big value 4095)
      // We read more than one time to be sure (4 times in this case)
      if (!actuator[n][FOUND] && (analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN])) / 4 >= 4095)
      {
        ledcWrite(actuator[n][PWM_CHANNEL], 0);
        actuator[n][CURRENT_POSITION] = 0;
        actuator[n][FOUND] = true;
        found++;
      }
    }
  }
  // Move all actuators to minimum
  // =============================
  for (int n = 0; n < NUMBERACTUATORS; n++)
  {
    digitalWrite (actuator[n][DIRECTION_PIN], DOWN);
    ledcWrite(actuator[n][PWM_CHANNEL], CALIBRATION_SPEED);
    actuator[n][FOUND] = false;
  }
  found = 0;
  while (found < NUMBERACTUATORS)
  {
    for (int n = 0; n < NUMBERACTUATORS; n++)
    {  

        Serial.print("n);
        Serial.print(" SEARCH MIN! Current position= ");
        Serial.println(actuator[n][CURRENT_POSITION]

      // Read limit switch position (we are looking for zero)
      if (!actuator[n][FOUND] && (analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN])) <= 0)
      {
        ledcWrite(actuator[n][PWM_CHANNEL], 0);
        actuator[n][MAXIMUM_POSITION] = -actuator[n][CURRENT_POSITION] / 2;
        actuator[n][MINIMUM_POSITION] = -actuator[n][MAXIMUM_POSITION];
        actuator[n][CURRENT_POSITION] = actuator[n][MINIMUM_POSITION];
        actuator[n][FOUND] = true;
        found++;

        // Send info to serial monitor for debug (use 500000 in serial speed to work with the Arduino IDE)
        Serial.println();
        Serial.print("Actuator ");
        Serial.print(n);
        Serial.print(":  Minimum=");
        Serial.print(actuator[n][MINIMUM_POSITION]);
        Serial.print("  Maximum=");
        Serial.println(actuator[n][MAXIMUM_POSITION]);
      }
    }
  }
}


// ======================
// Update actuator action
// ======================
// Here we calculate the PID depending of desired vs current position and send respective signal to motor controller
void updateActuatorSpeed(byte n)
{
  // Calculate PID
  // =============
  error = actuator[n][PRETENDED_POSITION] - actuator[n][CURRENT_POSITION];
  actuator[n][INTEGRATED_ERROR] += error;
  p = actuator[n][P_PID] * error;
  i = actuator[n][I_PID] * constrain(actuator[n][INTEGRATED_ERROR], -255, 255);
  d = actuator[n][D_PID] * (error - actuator[n][LAST_ERROR]);
  actuator[n][LAST_ERROR] = error;
  actuator[n][SPEED] = FlyPT_Constrain((p + i + d) >> 8, -255, 255); // 2 4 8 16 32 64 128 256 Divide by 256
 
  // Apply changes to motor controller
  // =================================
  if (actuator[n][SPEED] > 0)
  {
    ledcWrite(actuator[n][PWM_CHANNEL], actuator[n][SPEED]);
    digitalWrite (actuator[n][DIRECTION_PIN], UP);
  }
  else
  {
    ledcWrite(actuator[n][PWM_CHANNEL], -actuator[n][SPEED]);
    digitalWrite (actuator[n][DIRECTION_PIN], DOWN);
  }
 
  // Check limits
  // ============
  // Adjust position if minimum or maximum switch is activated in case of a drift in position calculation
  limitValue = (analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN]) + analogRead(actuator[n][LIMITS_PIN])) >> 2;
  // If they are actuated, recalibrate position
  if (limitValue >= 4095) actuator[n][CURRENT_POSITION] = actuator[n][MAXIMUM_POSITION]; // We are at maximum position
  else if (limitValue <= 0) actuator[n][CURRENT_POSITION] = actuator[n][MINIMUM_POSITION]; // We are at minimum position
}


// ======================================================
// Replacement for constrain function (seems to be buggy)
// ======================================================
int FlyPT_Constrain(int value, int low, int high)
{
  return (value < low) ? low : (value > high) ? high : value;
}


// ====================================
// Update current position of actuators
// ====================================
void updateCurrentPosition(byte n)
{
  int angle = digitalRead(actuator[n][HALL_1_PIN]) * 4 + digitalRead(actuator[n][HALL_2_PIN]) * 2 + digitalRead(actuator[n][HALL_3_PIN]);
  if (actuator[n][LAST_SUM] > 0) actuator[n][LAST_SUM] = stepUp[angle][actuator[n][LAST_ANGLE]];
  else actuator[n][LAST_SUM] = stepDown[angle][actuator[n][LAST_ANGLE]];
  actuator[n][LAST_ANGLE] = angle;
  actuator[n][CURRENT_POSITION] += actuator[n][LAST_SUM];
}


// ===========================
// Main loop running on core 1
// ===========================
// Here we get the simtools commands and make the actuator move
void loop()
{
  if (Serial.available())
  {
    bufferPrevious = bufferCurrent; // Store previous byte
    bufferCurrent = Serial.read(); // Get the new byte
    bufferCommand[bufferCount] = bufferCurrent; // Put the new byte in the array
    bufferCount++; // Change to next position in the array
    if(bufferCurrent==255 && bufferPrevious==255) bufferCount = 0; // Two 255 in sequence are the start of the position info
    if(bufferCount==6) // Having 6 bytes means we have the 3 positions and that we can update the pretended position
    {
      actuator[0][PRETENDED_POSITION] = map((bufferCommand[0] * 256) + bufferCommand[1], 0, MAXIMUM_BIT_VALUE, actuator[0][MINIMUM_POSITION], actuator[0][MAXIMUM_POSITION]);
      //actuator[1][PRETENDED_POSITION] = map((bufferCommand[2] * 256) + bufferCommand[3], 0, MAXIMUM_BIT_VALUE, actuator[1][MINIMUM_POSITION], actuator[1][MAXIMUM_POSITION]);
      //actuator[2][PRETENDED_POSITION] = map((bufferCommand[4] * 256) + bufferCommand[5], 0, MAXIMUM_BIT_VALUE, actuator[2][MINIMUM_POSITION], actuator[2][MAXIMUM_POSITION]);
      bufferCount=0;
    }
  }

  // Update orders sent to motor driver
  updateActuatorSpeed(0);
  //updateActuatorSpeed(1);
  //updateActuatorSpeed(2);
}


// ==============================================
// Update positions thread loop running in core 0
// ==============================================
void TaskUpdatePositions(void * parameter)
{
  while (true)
  {
      updateCurrentPosition(0);
      //updateCurrentPosition(1);
      //updateCurrentPosition(2);
  }
}

And show me the output of the serial monitor.
It's already for 1 actuator at 115200
It should show the position while it searches for her.

Do you have a separated motor to try it without switches?

Depending on result, it's the next step.

@Flymen ,

You can also test just the switches with this code:

Code:

SPECIAL TEST FOR SWITCHES IN FIRST ACTUATOR


// =======
// Defines
// =======
#define P 256                   // Proportional
#define I 0                     // Integral
#define D 256                   // Diferential
#define SERIAL_SPEED 115200     // Define to set up the serial speed comunication
#define CALIBRATION_SPEED 50    // Speed used to search for minimum and maximum values (0 to 255) if calibration is to fast or to slow, adjust this value
#define MAXIMUM_BIT_VALUE 32767 // Size of position values received (15 bits, above this, we use it to send other controls/requests besides position)
#define UP HIGH                 // Define to set up direction as HIGH
#define DOWN LOW                // Define to set down direction as LOW
#define PWM_RESOLUTION 8        // 0 to 255
#define PWM_FREQUENCY 1000      // The 8015A accepts up to 1 KHz
#define LIMITS_PIN 0            // Array index of pin to capture position from pot wich defines the limits
#define HALL_1_PIN 1            // Array index of pin used for hall 1 interrupt
#define HALL_2_PIN 2            // Array index of pin used for hall 2 interrupt
#define HALL_3_PIN 3            // Array index of pin used for hall 3 interrupt
#define LAST_ANGLE 4            // Array index of last angle that was detected
#define LAST_SUM 5              // Array index of last sum to know last direction it took
#define DIRECTION_PIN 6         // Array index of Pin to se direction
#define SPEED_PIN 7             // Array index of Pin to set speed with PWM
#define PWM_CHANNEL 8           // Array index of channel used to generate PWM
#define MINIMUM_POSITION 9      // Array index of minimum position
#define MAXIMUM_POSITION 10     // Array index of maximum position
#define CURRENT_POSITION 11     // Array index of current position
#define PRETENDED_POSITION 12   // Array index of pretended position
#define P_PID 13                // Array index of P from PID
#define I_PID 14                // Array index of I from PID
#define D_PID 15                // Array index of D from PID
#define INTEGRATED_ERROR 16     // Array index of sum of differences between pretended and current position
#define LAST_ERROR 17           // Array index of last difference between pretended and current position
#define SPEED 18                // Array index of speed of motor rotation
#define FOUND 19                // Array index of foud, used for in actuator calibration
#define NUMBERACTUATORS 1       // Number of actuators (for one board, please don't change, current code does not support less than 3 actuators. To use less, just disconnect them at hardware level)



static int actuator[3][20] = {
//  00  01  02  03  04  05  06  07  08  09  10  11  12  13  14  15  16  17  18  19
  {  4, 27, 26, 25,  5,  0, 32, 33,  0, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 },  // Actuator 1
  { 36, 35, 34, 39,  5,  0, 18,  5,  1, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 },  // Actuator 2
  { 15, 23, 22, 21,  5,  0, 19, 17,  2, -1, -1,  0,  0,  P,  I,  D,  0,  0,  0,  0 }   // Actuator 3
};


static int bufferPrevious = 0;      // To hold previous read fom serial command
static int bufferCurrent = 0;       // To hold current read fom serial command
static int bufferCount = 0;         // To hold current position in bufferCommand array
static int bufferCommand[6] = {0};  // To hold received info from serial



static int limitValue = 1023;
static int error = 0;
static int p = 0;
static int i = 0;
static int d = 0;


static int stepUp[8][8] = {
  {  0,  0,  0,  0,  0,  0,  0,  0},
  {  0,  0, -2, -1,  2,  1,  3,  0},
  {  0,  2,  0,  1, -2,  3, -1,  0},
  {  0,  1, -1,  0,  3,  2, -2,  0},
  {  0, -2,  2,  3,  0, -1,  1,  0},
  {  0, -1,  3, -2,  1,  0,  2,  0},
  {  0,  3,  1,  2, -1, -2,  0,  0},
  {  0,  0,  0,  0,  0,  0,  0,  0}
};
static int stepDown[8][8] = {
  {  0,  0,  0,  0,  0,  0,  0,  0},
  {  0,  0, -2, -1,  2,  1, -3,  0},
  {  0,  2,  0,  1, -2, -3, -1,  0},
  {  0,  1, -1,  0, -3,  2, -2,  0},
  {  0, -2,  2, -3,  0, -1,  1,  0},
  {  0, -1, -3, -2,  1,  0,  2,  0},
  {  0, -3,  1,  2, -1, -2,  0,  0},
  {  0,  0,  0,  0,  0,  0,  0,  0}
};


// ==============
// Initialization
// ==============
void setup()
{
  // Disable watchdog in both processors
  disableCore0WDT();
  disableCore1WDT();

  // Setup serial communication
  Serial.begin(SERIAL_SPEED, SERIAL_8N1);

  // Initialize
  for (int n = 0; n < NUMBERACTUATORS; n++)
  {
    // Initialize pins
    pinMode(actuator[n][LIMITS_PIN], INPUT);
    pinMode(actuator[n][HALL_1_PIN], INPUT);
    pinMode(actuator[n][HALL_2_PIN], INPUT);
    pinMode(actuator[n][HALL_3_PIN], INPUT);
    pinMode(actuator[n][DIRECTION_PIN], OUTPUT);
    pinMode(actuator[n][SPEED_PIN], OUTPUT);

    // Setup PWM
    ledcSetup(actuator[n][PWM_CHANNEL], PWM_FREQUENCY, PWM_RESOLUTION);
    ledcAttachPin(actuator[n][SPEED_PIN], actuator[n][PWM_CHANNEL]);
    ledcWrite(actuator[n][PWM_CHANNEL], 0);
  }
}



void loop()
{
   limitValue = (analogRead(actuator[0][LIMITS_PIN]) + analogRead(actuator[0][LIMITS_PIN]) + analogRead(actuator[0][LIMITS_PIN]) + analogRead(actuator[0][LIMITS_PIN])) >> 2;
   Serial.println(limitValue );
}

It should show for the first actuator, the value we receive from the switches.
If you press the min switch you get zero
If you press the max, you get 4093
If you don't press the switches, you should get something in the middle