Skip to content

Commit d8cf819

Browse files
facchinmfacchinm
committed
PID: use floats to control gains
1 parent 3200840 commit d8cf819

File tree

13 files changed

+1150
-49
lines changed

13 files changed

+1150
-49
lines changed

‎extras/D11-Firmware/Common.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ enum Commands {
2626
GET_INTERNAL_TEMP,
2727
CLEAR_IRQ,
2828
GET_FREE_RAM,
29+
GET_PID_VAL
2930
};
3031

3132
enum IRQCause {

‎extras/D11-Firmware/D11-Firmware.ino‎

Lines changed: 31 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,17 @@ void led_on() {
2626
digitalWrite(LED_BUILTIN, HIGH);
2727
}
2828

29+
typedef struct {
30+
Fix16 P;
31+
Fix16 I;
32+
Fix16 D;
33+
} PIDGains;
34+
35+
union PIDData {
36+
Fix16 txFloat;
37+
uint8_t txArray[4];
38+
} PIDGain;
39+
2940
void setup() {
3041

3142
WDT->CTRL.reg &= ~WDT_CTRL_ENABLE;
@@ -46,7 +57,7 @@ void setup() {
4657
encoders[1] = new EncoderWrapper(ENCODER_1_PIN_A, ENCODER_1_PIN_B, 0);
4758

4859
pid_control[0] = new PIDWrapper(encoders[0]->position, encoders[0]->velocity, dcmotors[0], 0, 10, 100); //10ms period velo, 100ms period pos
49-
pid_control[1] = new PIDWrapper(encoders[1]->position, encoders[1]->velocity, dcmotors[1], 1, 10, 100),
60+
pid_control[1] = new PIDWrapper(encoders[1]->position, encoders[1]->velocity, dcmotors[1], 1, 10, 100);
5061

5162
Wire.begin(I2C_ADDRESS);
5263
Wire.onRequest(requestEvent);
@@ -101,7 +112,7 @@ void receiveEvent(int howMany) {
101112
return;
102113
}
103114

104-
uint8_t buf[8];
115+
uint8_t buf[12];
105116
int i = 0;
106117
while (Wire.available() && i < sizeof(buf)) {
107118
buf[i++] = (uint8_t)Wire.read();
@@ -135,13 +146,10 @@ void receiveEvent(int howMany) {
135146
break;
136147
case SET_PID_GAIN_CL_MOTOR:
137148
{
138-
int16_t P16 = *((int16_t*)&buf[0]);
139-
int16_t I16 = *((int16_t*)&buf[2]);
140-
int16_t D16 = *((int16_t*)&buf[4]);
141-
Fix16 P = ((Fix16)P16) / short(1000);
142-
Fix16 I = ((Fix16)I16) / short(1000);
143-
Fix16 D = ((Fix16)D16) / short(1000);
144-
pid_control[target]->setGains(P, I , D);
149+
Fix16 P = *((Fix16*)&buf[0]);
150+
Fix16 I = *((Fix16*)&buf[4]);
151+
Fix16 D = *((Fix16*)&buf[8]);
152+
pid_control[target]->setGains(P, I, D);
145153
break;
146154
}
147155
case RESET_PID_GAIN_CL_MOTOR:
@@ -151,10 +159,10 @@ void receiveEvent(int howMany) {
151159
pid_control[target]->setControlMode((cl_control)value);
152160
break;
153161
case SET_POSITION_SETPOINT_CL_MOTOR:
154-
pid_control[target]->setSetpoint(TARGET_POSITION, Fix16(value * 1.0));
162+
pid_control[target]->setSetpoint(TARGET_POSITION, Fix16(value * 1.0)); //Change to integer. "value" is a 32 bit data type in this case (int).
155163
break;
156164
case SET_VELOCITY_SETPOINT_CL_MOTOR:
157-
pid_control[target]->setSetpoint(TARGET_VELOCITY, Fix16(value * 1.0));
165+
pid_control[target]->setSetpoint(TARGET_VELOCITY, Fix16(value * 1.0)); //Change to integer
158166
break;
159167
case SET_MAX_ACCELERATION_CL_MOTOR: {
160168
pid_control[target]->setMaxAcceleration(Fix16(value * 1.0));
@@ -211,6 +219,18 @@ void requestEvent() {
211219
break;
212220
case GET_FREE_RAM:
213221
Wire.write((int)FreeRam());
222+
break;
223+
case GET_PID_VAL:
224+
Fix16 gains[3];
225+
pid_control[target]->getGains((Fix16*)gains);
226+
227+
PIDGains pidGains;
228+
pidGains.P = gains[0];
229+
pidGains.I = gains[1];
230+
pidGains.D = gains[2];
231+
232+
Wire.write((uint8_t*)&pidGains, sizeof(pidGains));
233+
214234
break;
215235
}
216236
interrupts();

‎extras/D11-Firmware/PID.cpp‎

Lines changed: 8 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -12,25 +12,20 @@ void calculatePID_wrapper(void* arg) {
1212

1313
if (obj[i]->mode == CL_POSITION) {
1414
if (obj[i]->pid_pos->Compute()) {
15-
//slew limit velocity command with max accel
1615
Fix16 curvelocmd = obj[i]->velocmd;
17-
if ((prevvelocmd[i] - curvelocmd) > obj[i]->maxAcceleration) curvelocmd = prevvelocmd[i] - obj[i]->maxAcceleration;//limit decel
18-
if ((curvelocmd - prevvelocmd[i]) > obj[i]->maxAcceleration) curvelocmd = prevvelocmd[i] + obj[i]->maxAcceleration;//limit accel
1916
//copy position PID output to velocity PID setpoint
2017
obj[i]->targetvelo = curvelocmd;
2118
//save curvelocmd for next iteration
2219
prevvelocmd[i] = curvelocmd;
20+
int dutyout = (int16_t)curvelocmd ;
21+
obj[i]->motor->setDuty(dutyout); //dutyout should be a value between -100 and 100.
22+
}
23+
} else {
24+
//CL_VELOCITY
25+
if (obj[i]->pid_velo->Compute()) {
26+
int dutyout = int32_t(obj[i]->actualDuty);
27+
obj[i]->motor->setDuty(dutyout);
2328
}
24-
}
25-
26-
if (obj[i]->pid_velo->Compute()) {
27-
int dutyout = int32_t(obj[i]->actualDuty);
28-
//obj[i]->motor->setDuty(dutyout); not working so using line below instead
29-
30-
//deadzone compensation
31-
if (dutyout > 0) dutyout += 10;
32-
if (dutyout < 0) dutyout -= 10;
33-
obj[i]->motor->setDuty(dutyout);
3429
}
3530
}
3631
}

‎extras/D11-Firmware/PID.h‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,20 @@ class PIDWrapper {
3535

3636
void resetGains();
3737

38+
39+
void getGains(Fix16* gains) {
40+
if (this->mode == CL_VELOCITY) {
41+
gains[0] = pid_velo->GetKp();
42+
gains[1] = pid_velo->GetKi();
43+
gains[2] = pid_velo->GetKd();
44+
}
45+
if (this->mode == CL_POSITION) {
46+
gains[0] = pid_pos->GetKp();
47+
gains[1] = pid_pos->GetKi();
48+
gains[2] = pid_pos->GetKd();
49+
}
50+
};
51+
3852
void setControlMode(cl_control mode) {
3953
this->mode = mode;
4054
run();

‎extras/D11-Firmware/src/PID/PID_v1.cpp‎

Lines changed: 11 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -65,31 +65,25 @@ bool PID::Compute()
6565
/*Compute all the working error variables*/
6666
Fix16 input = *myInput;
6767
Fix16 error = *mySetpoint - input;
68-
Fix16 dInput = (input - lastInput);
69-
outputSum+= (ki * error);
68+
Fix16 dError = (error - lastError);
69+
iError += error; //add error to the integral term
7070

71-
/*Add Proportional on Measurement, if P_ON_M is specified*/
72-
if(!pOnE) outputSum-= kp * dInput;
71+
Fix16 Pout = kp * error;
72+
Fix16 Iout = ki * iError;
73+
Fix16 Dout = kd * dError;
7374

74-
if(outputSum > outMax) outputSum= outMax;
75-
else if(outputSum < outMin) outputSum= outMin;
75+
Fix16 output;
76+
output = Pout + Iout + Dout;
7677

77-
/*Add Proportional on Error, if P_ON_E is specified*/
78-
Fix16 output;
79-
if(pOnE) output = kp * error;
80-
else output = Fix16(0.0);
81-
82-
/*Compute Rest of PID Output*/
83-
output += outputSum - kd * dInput;
84-
85-
if(output > outMax) output = outMax;
78+
if(output > outMax) output = outMax;
8679
else if(output < outMin) output = outMin;
87-
*myOutput = output;
80+
*myOutput = output;
8881

8982
/*Remember some variables for next time*/
9083
lastInput = input;
9184
lastTime = now;
92-
return true;
85+
lastError = error;
86+
return true;
9387
}
9488
else return false;
9589
}

‎extras/D11-Firmware/src/PID/PID_v1.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,8 @@ class PID
8585

8686
unsigned long lastTime;
8787
Fix16 outputSum, lastInput;
88+
Fix16 lastError;
89+
Fix16 iError; //integral error
8890

8991
unsigned long SampleTime;
9092
Fix16 outMin, outMax;

‎src/ArduinoMotorCarrier.cpp‎

Lines changed: 22 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,13 +17,14 @@
1717
#include "ArduinoMotorCarrier.h"
1818

1919
namespace mc {
20-
void setDataPIDGains(Commands cmd, uint8_t target, int16_t P, int16_t I, int16_t D) {
20+
//Set Data (gains) with Fix16 format
21+
void setDataPIDGains(Commands cmd, uint8_t target, Fix16 P, Fix16 I, Fix16 D) {
2122
Wire.beginTransmission(I2C_ADDRESS);
2223
Wire.write((uint8_t)cmd);
2324
Wire.write((uint8_t)target);
24-
Wire.write((uint8_t*)&P, 2);
25-
Wire.write((uint8_t*)&I, 2);
26-
Wire.write((uint8_t*)&D, 2);
25+
Wire.write((uint8_t*)&P, 4);
26+
Wire.write((uint8_t*)&I, 4);
27+
Wire.write((uint8_t*)&D, 4);
2728
Wire.endTransmission();
2829
}
2930

@@ -35,6 +36,23 @@ void setData(Commands cmd, uint8_t target, int data) {
3536
Wire.endTransmission();
3637
}
3738

39+
int getDataPIDGains(Commands cmd, uint8_t target, uint8_t* buf, int dataSize) {
40+
Wire.beginTransmission(I2C_ADDRESS);
41+
Wire.write((uint8_t)cmd);
42+
Wire.write((uint8_t)target);
43+
Wire.endTransmission();
44+
45+
int i = 0;
46+
Wire.requestFrom(I2C_ADDRESS, dataSize + 1); //one extra for the irq_status
47+
uint8_t status = Wire.read();
48+
if (status != 0) controller.irq_status = status;
49+
50+
while (Wire.available()) {
51+
buf[i++] = (uint8_t)Wire.read();
52+
}
53+
return i;
54+
}
55+
3856
int getData(Commands cmd, uint8_t target, uint8_t* buf) {
3957
Wire.beginTransmission(I2C_ADDRESS);
4058
Wire.write((uint8_t)cmd);

‎src/ArduinoMotorCarrier.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "MotorController.h"
2626
#include "Common.h"
2727
#include "PID.h"
28+
#include "src/FpF.hpp"
2829

2930
extern mc::MotorController controller;
3031

‎src/Common.h‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ enum Commands {
4242
GET_INTERNAL_TEMP,
4343
CLEAR_IRQ,
4444
GET_FREE_RAM,
45+
GET_PID_VAL
4546
};
4647

4748
enum IRQCause {
@@ -70,10 +71,13 @@ enum IRQCause {
7071
#define IN4 A2
7172
#endif
7273

74+
#include "src/FpF.hpp"
75+
#define Fix16 mn::MFixedPoint::FpF32<8>
7376

7477
namespace mc {
7578
int getData(Commands cmd, uint8_t target, uint8_t* buf);
7679
void setData(Commands cmd, uint8_t target, int data);
77-
void setDataPIDGains(Commands cmd, uint8_t target, int16_t P, int16_t I, int16_t D);
80+
void setDataPIDGains(Commands cmd, uint8_t target, Fix16 P, Fix16 I, Fix16 D);
81+
int getDataPIDGains(Commands cmd, uint8_t target, uint8_t* buf, int dataSize);
7882
int getData(Commands cmd, uint8_t* buf);
7983
}

‎src/PID.cpp‎

Lines changed: 35 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -20,12 +20,23 @@
2020
namespace mc {
2121
static int next_instance = 0;
2222

23+
typedef struct {
24+
Fix16 P;
25+
Fix16 I;
26+
Fix16 D;
27+
} PIDGains;
28+
29+
union {
30+
Fix16 rxFloat = Fix16(0.0);
31+
uint8_t rxArray[4];
32+
} PIDGain;
33+
2334
PID::PID() {
2435
instance = next_instance;
2536
next_instance++;
2637
}
2738

28-
void PID::setGains(int16_t kp, int16_t ki, int16_t kd) {
39+
void PID::setGains(Fix16 kp, Fix16 ki, Fix16 kd) {
2940
setDataPIDGains(SET_PID_GAIN_CL_MOTOR, instance, kp, ki, kd);
3041
}
3142

@@ -42,7 +53,11 @@ void PID::setSetpoint(cl_target control_target, int target) {
4253
setData(SET_POSITION_SETPOINT_CL_MOTOR, instance, target);
4354
}
4455
if (control_target == TARGET_VELOCITY) {
45-
setData(SET_VELOCITY_SETPOINT_CL_MOTOR, instance, target);
56+
if (!target) {
57+
setData(SET_PWM_DUTY_CYCLE_DC_MOTOR, instance, target); //Fix target = 0 issue in PID_VELOCITY mode.
58+
} else {
59+
setData(SET_VELOCITY_SETPOINT_CL_MOTOR, instance, target);
60+
}
4661
}
4762
}
4863

@@ -57,6 +72,24 @@ void PID::setMaxVelocity(int maxVelocity) {
5772
void PID::setLimits(int16_t minDuty, int16_t maxDuty) {
5873
setData(SET_MIN_MAX_DUTY_CYCLE_CL_MOTOR, instance, (minDuty << 16 | maxDuty));
5974
}
75+
76+
Fix16 mc::PID::getPgain() {
77+
PIDGains pidGains;
78+
int stat = getDataPIDGains(GET_PID_VAL, instance, (uint8_t*)&pidGains ,sizeof(pidGains));
79+
return pidGains.P;
80+
}
81+
82+
Fix16 mc::PID::getIgain() {
83+
PIDGains pidGains;
84+
int stat = getDataPIDGains(GET_PID_VAL, instance, (uint8_t*)&pidGains ,sizeof(pidGains));
85+
return pidGains.I;
86+
}
87+
Fix16 mc::PID::getDgain() {
88+
PIDGains pidGains;
89+
int stat = getDataPIDGains(GET_PID_VAL, instance, (uint8_t*)&pidGains ,sizeof(pidGains));
90+
return pidGains.D;
91+
}
92+
6093
}
6194

6295
namespace d21 {

0 commit comments

Comments
 (0)