hallo allerseits,
ich versuche nicht nur den omni-move zum leben zu erwecken, sondern ihm auch respekt vor hindernissen beizubringen, mit mehr oder weniger erfolg. Er:
- fährt nach dem drücken der starttaste los
- losfährt nur dann, wenn kein hindernis in fahrtrichtung vorhanden ist
- dreht sich nach links, wenn ein hindernis beim start detektiert wird
- er detektiert aber keine hindernisse, während er fährt
- kommt aus dem drehen nicht mehr raus, obwohl eine drehung von nur 5° vorgegeben ist
Wahrscheinlich liegt es am setzen der variablen "hindernis" mit false, bzw. true und die damit verbundene reaktion zusammen. Inzwischen sehe ich aber vor lauter bäumen den wald nicht und find den fehler nicht. Könnte bitte jemand über den code drüber schauen?
vielen dank...Code:#include <CustomStepper.h> #include <NewPing.h> #include <LCD.h> #include <LiquidCrystal_I2C.h> #include <Bounce2.h> #include <Wire.h> #define TRIGGER_PIN 6 #define ECHO_PIN 7 #define MAX_DISTANCE 400 NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE); uint8_t idx; uint16_t distanz; uint16_t uS; enum stepper_e { stepper_HM, stepper_VR, stepper_VL, stepper_MAX }; CustomStepper stepper[stepper_MAX] { CustomStepper(35, 37, 39, 41),//stepper_HM-I CustomStepper(23, 25, 27, 29),//stepper_VR-II CustomStepper(47, 49, 51, 53) //stepper_VL-III }; boolean start_ping; boolean hindernis; boolean start; Bounce pin5 = Bounce(); void setup() { pinMode(5, INPUT_PULLUP); pin5.attach(5); pin5.interval(5); start_ping = false; hindernis = false; start = false; Serial.begin(115200); Serial1.begin(115200); lcd.begin(16, 2); lcd.clear(); lcd.setCursor(0, 0); lcd.setBacklight(HIGH); lcd.print("cruise_av_col_2"); lcd.setCursor(0, 1); lcd.print("start_taster"); delay(2000); lcd.clear(); lcd.setBacklight(LOW); } void loop() { if (pin5.update()) { if (pin5.fell()) { start = !start; } } if (start) { // fahrt_1(); hindernis_vorh(); if (hindernis == false) { vorwaerts(); Serial1.println("fahre richtung HM"); fahrt_ausfuehren(); } else { rotate_left_deg(); fahrt_ausfuehren(); } } } /***********************************************************/ void hindernis_vorh(void) { if (start_ping == false) ping_distanz(); if (uS != NO_ECHO) { if (((uS / US_ROUNDTRIP_CM) <= 25)) hindernis = true; } else { hindernis = false; } } void ping_distanz(void) { uS = sonar.ping(); Serial1.print("Ping: "); Serial1.print(uS / US_ROUNDTRIP_CM); Serial1.println(" cm"); Serial.print("Ping: "); Serial.print(uS / US_ROUNDTRIP_CM); Serial.println(" cm"); start_ping = true; } void alle_stepper_stop() { for (idx = stepper_HM; idx < stepper_MAX; idx++) { stepper[idx].setRPM(0); stepper[idx].setSPR(4075.7728395); stepper[idx].setDirection(STOP); } } void vorwaerts() { if (start_ping == true) ping_distanz(); if (hindernis == true) { /* Serial1.print(hindernis); Serial1.println(" hindernis - true - rotiere links- "); Serial.print(hindernis); Serial.println(" hindernis - true - rotiere links- "); */ rotate_left_deg(); fahrt_ausfuehren(); hindernis = false; } else hindernis = false; /* Serial.print(hindernis); Serial1.println(" freie fahrt - richtung HM"); Serial.print(hindernis); Serial.println(" freie fahrt - richtung HM"); */ richtung_HM(); fahrt_ausfuehren(); } void richtung_HM() { if (start_ping == true) ping_distanz(); if (hindernis == true) { /* Serial1.print(hindernis); Serial1.println(" hindernis - true - rotiere links- "); Serial.print(hindernis); Serial.println(" hindernis - true - rotiere links- "); */ rotate_left_deg(); fahrt_ausfuehren(); hindernis = false; } else { hindernis == false; stepper[stepper_HM].setRPM(12); stepper[stepper_VR].setRPM(12); stepper[stepper_VL].setRPM(12); stepper[stepper_HM].setSPR(4075.7728395); stepper[stepper_VR].setSPR(4075.7728395); stepper[stepper_VL].setSPR(4075.7728395); stepper[stepper_VL].setDirection(CCW); stepper[stepper_VL].rotate(2); stepper[stepper_VR].setDirection(CW); stepper[stepper_VR].rotate(2); } } void richtung_VL() { stepper[stepper_HM].setRPM(12); stepper[stepper_VR].setRPM(12); stepper[stepper_VL].setRPM(12); stepper[stepper_HM].setSPR(4075.7728395); stepper[stepper_VR].setSPR(4075.7728395); stepper[stepper_VL].setSPR(4075.7728395); stepper[stepper_VR].setDirection(CCW); stepper[stepper_VR].rotate(2); stepper[stepper_HM].setDirection(CW); stepper[stepper_HM].rotate(2); } void richtung_VR() { stepper[stepper_HM].setRPM(12); stepper[stepper_VR].setRPM(12); stepper[stepper_VL].setRPM(12); stepper[stepper_HM].setSPR(4075.7728395); stepper[stepper_VR].setSPR(4075.7728395); stepper[stepper_VL].setSPR(4075.7728395); stepper[stepper_VL].setDirection(CW); stepper[stepper_VL].rotate(2); stepper[stepper_HM].setDirection(CCW); stepper[stepper_HM].rotate(2); } void test_richtung() //VR { stepper[stepper_HM].setRPM(12); stepper[stepper_VR].setRPM(12); stepper[stepper_VL].setRPM(12); stepper[stepper_HM].setSPR(4075.7728395); stepper[stepper_VR].setSPR(4075.7728395); stepper[stepper_VL].setSPR(4075.7728395); stepper[stepper_VL].setDirection(CW); stepper[stepper_VL].rotate(2); stepper[stepper_HM].setDirection(CCW); stepper[stepper_HM].rotate(2); } void rotate_right_deg(void) { for (idx = stepper_HM; idx < stepper_MAX; idx++) { stepper[idx].setRPM(12); stepper[idx].setSPR(4075.7728395); stepper[idx].setDirection(CW); stepper[idx].rotateDegrees(60); } } void rotate_left_deg(void) { hindernis = false; for (idx = stepper_HM; idx < stepper_MAX; idx++) { stepper[idx].setRPM(12); stepper[idx].setSPR(4075.7728395); stepper[idx].setDirection(CW); stepper[idx].rotateDegrees(5); } } void rotate_right(void) { for (idx = stepper_HM; idx < stepper_MAX; idx++) { stepper[idx].setRPM(12); stepper[idx].setSPR(4075.7728395); stepper[idx].setDirection(CCW); stepper[idx].rotate(2); } } void rotate_left(void) { for (idx = stepper_HM; idx < stepper_MAX; idx++) { stepper[idx].setRPM(12); stepper[idx].setSPR(4075.7728395); stepper[idx].setDirection(CW); stepper[idx].rotate(2); } } boolean fahrt_fertig() { return stepper[stepper_HM].isDone() && stepper[stepper_VR].isDone() && stepper[stepper_VL].isDone(); } void fahrt_ausfuehren() { while ( ! fahrt_fertig() ) { for (idx = stepper_HM; idx < stepper_MAX; idx++) { stepper[idx].run(); // delay(1); } } } /************************************************************/ void fahrt_1() { rotate_right(); Serial1.println("drehe rechts"); fahrt_ausfuehren(); alle_stepper_stop(); fahrt_ausfuehren(); richtung_HM(); Serial1.println("fahre richtung HM"); fahrt_ausfuehren(); alle_stepper_stop(); fahrt_ausfuehren(); richtung_VR(); fahrt_ausfuehren(); alle_stepper_stop(); fahrt_ausfuehren(); richtung_VL(); fahrt_ausfuehren(); alle_stepper_stop(); fahrt_ausfuehren(); rotate_left(); fahrt_ausfuehren(); alle_stepper_stop(); fahrt_ausfuehren(); } void halt() { alle_stepper_stop(); fahrt_ausfuehren(); }
Zitieren
Lesezeichen