From c2fbf18814e30597f49390f00fa15a27e2c659ec Mon Sep 17 00:00:00 2001 From: mitchellhansen Date: Fri, 2 Oct 2020 11:36:33 -0700 Subject: [PATCH] tweak control algo --- dozer_main.cpp | 119 ++++++++++++++---------------------------------- remote_main.cpp | 16 ++++++- 2 files changed, 50 insertions(+), 85 deletions(-) diff --git a/dozer_main.cpp b/dozer_main.cpp index a0c2453..e5e3897 100644 --- a/dozer_main.cpp +++ b/dozer_main.cpp @@ -45,16 +45,9 @@ struct StickValues { #define L_TRK_FWD 17 #define R_TRK_BKW 19 #define L_TRK_BKW 16 -#define BKT_FWD 14 -#define BKT_BKW 15 +#define BKT_FWD 15 +#define BKT_BKW 14 -#define PI 3.141592654 -#define DEG(rad) (rad*180.0/PI) - -#define FULL_SPEED_ZONE 5 -#define MAGIC_45_2_100 2.22222 -#define MAG_SCALER_H 200 -#define MAG_SCALER_L 290 int main() { init(); @@ -67,6 +60,13 @@ int main() { SoftPWMBegin(); + SoftPWMSet(R_TRK_FWD, 0); // bucket + SoftPWMSet(L_TRK_FWD, 0); // bucket + SoftPWMSet(R_TRK_BKW, 0); // left track backward + SoftPWMSet(L_TRK_BKW, 0); // left track forward + SoftPWMSet(BKT_FWD , 0); // right track backward + SoftPWMSet(BKT_BKW , 0); // right track forward + RF24 radio(CE_PIN, CS_PIN); radio.begin(); radio.setPALevel (RF24_PA_MAX); @@ -116,10 +116,10 @@ int main() { radio.flush_rx(); - if (stick_values.stick_1_y > 10) { + if (stick_values.stick_2_y > 10) { digitalWrite(GREEN, HIGH); digitalWrite(RED, LOW); - } else if (stick_values.stick_1_y < -10) { + } else if (stick_values.stick_2_y < -10) { digitalWrite(RED, HIGH); digitalWrite(GREEN, LOW); } else { @@ -175,45 +175,32 @@ int main() { left_track = ((270 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H); } -// if (ang >= 0 && ang < 90 && mag > 1) { -// if (ang < 0 + FULL_SPEED_ZONE) -// right_track = left_track = -100; -//// else if (ang > 90 - FULL_SPEED_ZONE) -//// right_track = left_track = -100; -// else -// right_track = ((270 + 45) - ang) * 2 * (mag/290); -// -// left_track = -100 * (int)(mag/290); -// } -// if (ang >= 90 && ang < 180 && mag > 1) { -// right_track = 100 * (mag/290); -// if (ang > 90 - FULL_SPEED_ZONE) { -// right_track = 100; -// left_track = -100; -// } -// else -// left_track = (135 - ang) * -2 * (mag/290); -// } -// if (ang >= 180 && ang < 270 && mag > 1) { -// if (ang > 180 - FULL_SPEED_ZONE) -// right_track = 100 * (mag/290); -// else -// right_track = ((180 + 45) - ang) * 2 * (mag/290); -// left_track = 100 * (mag/290); -// } -// if (ang >= 270 && ang <= 360 && mag > 1) { -// right_track = -100 * (mag/290); -// if (ang > 270 - FULL_SPEED_ZONE) { -// left_track = -100 * (mag/290); -// } else { -// left_track = ((270 + 45) - ang) * 2 * (mag/290); -// } -// } + if (stick_values.stick_2_y > 5) { + SoftPWMSet(BKT_FWD, 255); + SoftPWMSet(BKT_BKW, 0); + //SoftPWMSet(BKT_FWD, max(0, min(255, abs(stick_values.stick_2_y)))); + //SoftPWMSet(R_TRK_BKW, max(0, min(255, abs(stick_values.stick_2_y)))); + // SoftPWMSet(BKT_BKW, 0); + } else if (stick_values.stick_2_y < -5) { + SoftPWMSet(BKT_BKW, 255); + SoftPWMSet(BKT_FWD, 0); + //SoftPWMSet(BKT_BKW, max(0, min(255, abs(stick_values.stick_2_y)))); + //SoftPWMSet(R_TRK_FWD, max(0, min(255, abs(stick_values.stick_2_y)))); + //SoftPWMSet(BKT_, 1); + } else { + SoftPWMSet(BKT_BKW, 0); + SoftPWMSet(BKT_FWD, 0); + } + + left_track = left_track * 3; + right_track = right_track * 3; if (left_track > 0) { SoftPWMSet(L_TRK_FWD, max(0, min(255, left_track))); + SoftPWMSet(L_TRK_BKW, 0); } else if (left_track < 0) { SoftPWMSet(L_TRK_BKW, max(0, min(255, abs(left_track)))); + SoftPWMSet(L_TRK_FWD, 0); } else if (left_track == 0) { SoftPWMSet(L_TRK_BKW, 0); SoftPWMSet(L_TRK_FWD, 0); @@ -221,53 +208,17 @@ int main() { if (right_track > 0) { SoftPWMSet(R_TRK_FWD, max(0, min(255, right_track))); + SoftPWMSet(R_TRK_BKW, 0); } else if (right_track < 0) { SoftPWMSet(R_TRK_BKW, max(0, min(255, abs(right_track)))); + SoftPWMSet(R_TRK_FWD, 0); } else if (right_track == 0) { SoftPWMSet(R_TRK_FWD, 0); SoftPWMSet(R_TRK_BKW, 0); } -// int forward_delta = 0; -// if (stick_values.stick_1_x > 10) { -// forward_delta = min(max(0, stick_values.stick_1_x), 255); -// -// } else if (stick_values.stick_1_x < -10) { -// forward_delta = min(max(-255, stick_values.stick_1_x), 0); -// } -// -// if (stick_values.stick_1_y > 10) { -// int delta = min(max(0, stick_values.stick_1_y), 255); -// if (forward_delta > 0) { -// SoftPWMSet(R_TRK_FWD, delta + forward_delta); -// } else if (forward_delta <= 0) { -// SoftPWMSet(R_TRK_BKW, delta + forward_delta); -// } -// SoftPWMSet(L_TRK_FWD, delta); -// } else if (stick_values.stick_1_y < -10) { -// int delta = min(max(0, abs(stick_values.stick_1_y)), 255); -// if (forward_delta > 0) { -// SoftPWMSet(L_TRK_FWD, delta + forward_delta); -// } else if (forward_delta <= 0) { -// SoftPWMSet(L_TRK_BKW, delta + forward_delta); -// } -// SoftPWMSet(R_TRK_FWD, delta); -// } else { -// -// if (forward_delta > 10) { -// SoftPWMSet(L_TRK_FWD, forward_delta); -// SoftPWMSet(R_TRK_FWD, forward_delta); -// } else if (forward_delta < 10) { -// SoftPWMSet(L_TRK_BKW, forward_delta); -// SoftPWMSet(R_TRK_BKW, forward_delta); -// } else { -// SoftPWMSet(R_TRK_FWD, 0); -// SoftPWMSet(R_TRK_BKW, 0); -// SoftPWMSet(L_TRK_BKW, 0); -// SoftPWMSet(L_TRK_FWD, 0); -// } -// } + if (serialEventRun) serialEventRun(); diff --git a/remote_main.cpp b/remote_main.cpp index d45372e..b3df089 100644 --- a/remote_main.cpp +++ b/remote_main.cpp @@ -57,7 +57,8 @@ int main() { radio.openReadingPipe(1, dozer_pipe); radio.startListening(); - delay(1000); + pinMode(9, INPUT_PULLUP); + delay(3000); int16_t ref_stick_1_y = analogRead(A4); // RY int16_t ref_stick_1_x = analogRead(A5); // RX @@ -66,6 +67,17 @@ int main() { while (true) { + int val = digitalRead(9); // read input value + Serial.println(val); + if (val == 0) { // check if the input is HIGH (button released) + ref_stick_1_y = analogRead(A4); // RY + ref_stick_1_x = analogRead(A5); // RX + ref_stick_2_y = analogRead(A6); // LY + ref_stick_2_x = analogRead(A7); // LX + } else { + + } + // Parse into a struct StickValues stick_values; stick_values.stick_1_y = analogRead(A5); // RY @@ -144,6 +156,8 @@ int main() { Serial.print(left_track); Serial.print(", "); Serial.print(right_track); + Serial.print(", "); + Serial.print(stick_values.stick_2_y); Serial.println("]"); //Serial.println("");