Hi everyone. I’m trying to write a program to control a linear actuator based on input from a motion control program. The problem is when the ‘Input’ variable is assigned a fixed value for troubleshooting the program works. When the ‘Input’ is less than ‘Setpoint’ the motor turns in one direction and when ‘Input’ is higher than ‘Setpoint’ the motor turns in the opposite direction. However when ‘Input’ gets its value from the sensor and the map() function is used it doesn’t work. The value of ‘Input’ is always seems higher than ‘Setpoint’ and the only turns in one direction. I’ve used another program to test the sensor output using the map() function and it seems to have a valid output in the serial monitor. Any suggestions would be helpful.
Thanks.
#include <PID_v1.h>
#include <Wire.h>
#include <Adafruit_AS5600.h>
Adafruit_AS5600 sensor;
//int RPWM_Output = 2; // Arduino PWM output pin 5; connect to IBT-2 pin 2 (RPWM)
//int LPWM_Output = 3; // Arduino PWM output pin 6; connect to IBT-2 pin 3 (LPWM)
int cr, count = 1, pin, c, time = 5, retlimit = 1, exlimit = 4095;
int sread[9];
//Define Variables we'll be connecting to
double Input, Output1, Output2, Output3, Output4, Output5, Output6, Setpoint, gap;
//Define the aggressive and conservative Tuning Parameters
double aggKp = 1.2, aggKi = 0.070, aggKd = 0.365;
double consKp = 0.125, consKi = 0.015, consKd = 0.08;
//Specify the links and initial tuning parameters
PID act1PID(&Input, &Output1, &Setpoint, consKp, consKi, consKd, DIRECT);
PID act2PID(&Input, &Output2, &Setpoint, consKp, consKi, consKd, DIRECT);
PID act3PID(&Input, &Output3, &Setpoint, consKp, consKi, consKd, DIRECT);
PID act4PID(&Input, &Output4, &Setpoint, consKp, consKi, consKd, DIRECT);
PID act5PID(&Input, &Output5, &Setpoint, consKp, consKi, consKd, DIRECT);
PID act6PID(&Input, &Output6, &Setpoint, consKp, consKi, consKd, DIRECT);
void setup() {
Serial.begin(115200);
for (c = 2; c < 14; c++) {
pinMode(c, OUTPUT);
}
act1PID.SetMode(AUTOMATIC);
act1PID.SetSampleTime(time);
act2PID.SetMode(AUTOMATIC);
act2PID.SetSampleTime(time);
act3PID.SetMode(AUTOMATIC);
act3PID.SetSampleTime(time);
act4PID.SetMode(AUTOMATIC);
act4PID.SetSampleTime(time);
act5PID.SetMode(AUTOMATIC);
act5PID.SetSampleTime(time);
act6PID.SetMode(AUTOMATIC);
act6PID.SetSampleTime(time);
pinMode(LED_BUILTIN, OUTPUT);
}
void loop() {
if (Serial.available() > 0) {
cr = Serial.read();
if (cr == 65) {
do {
if (Serial.available() > 0) {
sread[count] = (Serial.read());
count++;
}
} while (count < 9);
}
}
if (sread[1] == 66) {
//--------------------------------------------------actuator 1
Setpoint = sread[3]; //Input from BFF motion software. 8 bit. range 0-255
//Setpoint=map(sread[3], 0, 255, retlimit, exlimit);
//Setpoint=((Setpoint+1)*2.35)+100;
Input = sensor.getAngle(); // Input from AS5600 sensor. 12 bit. 0-4095
Input = map(Input, 0, 4095, 0, 255); // Mapped to range 0-255
//Input = 120; Used for testing.
if (Setpoint > Input) {
act1PID.SetControllerDirection(DIRECT);
analogWrite(3, LOW);
pin = 2;
digitalWrite(LED_BUILTIN, LOW);
} else {
act1PID.SetControllerDirection(REVERSE);
analogWrite(2, LOW);
pin = 3;
//if (Input > 100 && Input < 150)
//{
digitalWrite(LED_BUILTIN, HIGH);
//}
}
gap = abs(Setpoint - Input); //distance away from setpoint
if (gap < 6) { //we're close to setpoint, use conservative tuning parameters
act1PID.SetTunings(consKp, consKi, consKd);
} else {
//we're far from setpoint, use aggressive tuning parameters
act1PID.SetTunings(aggKp, aggKi, aggKd);
}
act1PID.Compute();
if (gap < 2) {
Output1 = 0;
}
analogWrite(pin, 50);
count = 1;
}
}
