89 lines
2.0 KiB
C++
89 lines
2.0 KiB
C++
#include <ArduPID.h>
|
|
|
|
#define MOT_DX 0
|
|
#define MOT_SX 1
|
|
|
|
constexpr float MOT_SX_MULT = 1.0;
|
|
constexpr float MOT_DX_MULT = 1.0;
|
|
|
|
ArduPID myController;
|
|
|
|
// Calculated with matlab, then adjusted by hand
|
|
|
|
/* With KP = 10, about 12 periods of oscillation in 1sec
|
|
Pc = 0.083
|
|
|
|
PID Kp = 0.6Kc, Ti = 0.5Pc Td = 0.125Pc
|
|
Ki = KpTc/ti
|
|
Kd = KpTd/Tc
|
|
*/
|
|
|
|
|
|
constexpr double KP = 5.8;
|
|
constexpr double KI = 0.2;
|
|
constexpr double KD = 0.1;
|
|
|
|
//double setpoint = -0.015;
|
|
double setpoint = 0.0;
|
|
double output = 0;
|
|
double input = 0;
|
|
|
|
double roll{ 0 }, pitch{ 0 }, yaw{ 0 };
|
|
|
|
constexpr double max_torque_sx = 0.392;
|
|
constexpr double max_torque_dx = 0.36;
|
|
constexpr double max_torque = max_torque_dx+max_torque_sx;
|
|
constexpr double torque_sx_coeff = max_torque_sx/max_torque;
|
|
constexpr double torque_dx_coeff = max_torque_dx/max_torque;
|
|
|
|
void setup(void) {
|
|
Serial.begin(9600);
|
|
delay(500);
|
|
|
|
setup_imu();
|
|
setup_motors();
|
|
|
|
pinMode(LED_BUILTIN, OUTPUT);
|
|
digitalWrite(LED_BUILTIN, HIGH);
|
|
|
|
// Let the initial error from madgwick filter discharge without affecting the integral term of the PID
|
|
unsigned long t = millis();
|
|
while (millis() - t < 3000) {
|
|
update_imu();
|
|
}
|
|
|
|
myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
|
|
myController.setOutputLimits(-max_torque, max_torque); // double of max torque motors can exhert
|
|
// myController.setWindUpLimits(-0.2 , 0.02);
|
|
myController.setSampleTime(1);
|
|
myController.start();
|
|
|
|
|
|
digitalWrite(LED_BUILTIN, LOW);
|
|
}
|
|
|
|
double map_double(double x, double in_min, double in_max, double out_min, double out_max) {
|
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
|
}
|
|
|
|
void loop() {
|
|
compute();
|
|
}
|
|
|
|
void compute(){
|
|
update_imu();
|
|
|
|
input = pitch;
|
|
|
|
myController.compute();
|
|
|
|
double torque = abs(output);
|
|
|
|
double s = sign(output);
|
|
double pwm_dx = torque <= 0.005 ? 30*s : torque_to_pwm_dx(torque*torque_dx_coeff)*s;
|
|
double pwm_sx = torque <= 0.005 ? 30*s : torque_to_pwm_sx(torque*torque_sx_coeff)*s;
|
|
|
|
move_pwm(MOT_SX, pwm_sx);
|
|
move_pwm(MOT_DX, pwm_dx);
|
|
}
|