mirror
/
R2Home
kopia lustrzana https://github.com/YohanHadji/R2Home
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Servo finally fixed..

pull/1/head
Mathis et Yohan 2021-12-10 20:08:53 +01:00
rodzic ed66f34f2f
commit 89f27037fd
1 zmienionych plików z 17 dodań i 14 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

27
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_SERVO/R2Home_SERVO.ino
Wyświetl plik

@ -3,7 +3,7 @@
// ------------------------------------------------------------------------------------ // // ------------------------------------------------------------------------------------ //
bool sim_signal = true; // Use this line to switch between simulated signal or Servo tester bool sim_signal = false; // Use this line to switch between simulated signal or Servo tester
// ------------------------------------------------------------------------------------ // // ------------------------------------------------------------------------------------ //
@ -16,15 +16,16 @@ ServoInputPin<InputPinCmd> inputCmd;
Servo outputServo; Servo outputServo;
float parallax_steer = 1500; float parallax_steer_a = 1500;
float parallax_steer_b = 1500;
float feedback_value = 1500; float feedback_value = 1500;
unsigned long tread = 0; unsigned long tread = 0;
unsigned long tchange = 0; unsigned long tchange = 0;
unsigned long tparallax = 0; unsigned long tparallax = 0;
float Kp = 1.5; float Kp = 0.75;
float Kd = 6; float Kd = 0;
float lasterror = 0; float lasterror = 0;
float error = 0; float error = 0;
@ -46,8 +47,8 @@ void setup() {
void loop() { void loop() {
if (sim_signal == true) { sim(3); } if (sim_signal == false){ getcmd(50); }
else { getcmd(50); } else { sim(3); }
updatecmd(200); updatecmd(200);
@ -60,8 +61,9 @@ void getcmd(int a) {
if ((millis()-tread)>=(1000/a)) { if ((millis()-tread)>=(1000/a)) {
tread = millis(); tread = millis();
parallax_steer = inputCmd.getPulse(); parallax_steer_a = inputCmd.getPulse();
parallax_steer = constrain(parallax_steer, 1000, 2000); if ((parallax_steer_a<900) or (parallax_steer_a>2100)) { parallax_steer_a = 1500; }
parallax_steer_a = constrain(parallax_steer_a, 1000, 2000);
} }
} }
@ -81,7 +83,7 @@ void sim(int a) {
toss = 0; toss = 0;
} }
} }
parallax_steer = sim_cmd; parallax_steer_a = sim_cmd;
} }
// ------------------ Update the command at a Hz ------------------ // // ------------------ Update the command at a Hz ------------------ //
@ -101,11 +103,13 @@ void updatecmd(int a) {
// ------------------------------------------------------------------------------------ // // ------------------------------------------------------------------------------------ //
parallax_steer = map(parallax_steer, 1000, 2000, 1100, 1900); // this is the ONLY line to change to reduce the range, for example, "[...] 1450, 1550)" parallax_steer_b = map(parallax_steer_a, 1000, 2000, 1100, 1900); // this is the ONLY line to change to reduce the range, for example, "[...] 1450, 1550)"
Serial.print(parallax_steer_b); Serial.print(","); Serial.println(feedback_value);
// ------------------------------------------------------------------------------------ // // ------------------------------------------------------------------------------------ //
error = (parallax_steer - feedback_value); error = (parallax_steer_b - feedback_value);
raterror = (error-lasterror); raterror = (error-lasterror);
PIDsum = (Kp*error)+(Kd*raterror); PIDsum = (Kp*error)+(Kd*raterror);
cmd = 1500 + PIDsum; cmd = 1500 + PIDsum;
@ -114,7 +118,6 @@ void updatecmd(int a) {
if (cmd <1500) {cmd = cmd-20;} if (cmd <1500) {cmd = cmd-20;}
constrain(cmd, 1280, 1720); constrain(cmd, 1280, 1720);
Serial.print(parallax_steer); Serial.print(","); Serial.println(feedback_value);
outputServo.writeMicroseconds(cmd); outputServo.writeMicroseconds(cmd);
lasterror = error; lasterror = error;
} }