ZitierenHallo zusammen,
ich verwende ebenfalls die erweiterte Lib (an dieser Stelle einmal vielen Dank dafür, dass ihr die zur Verfügung stellt) und habe dabei folgendes Problem:
Da ich zur sauberen Geradeausfahrt den Encoder nutze, muss ich auch die Kollisionsabfrage über Interrupts durchführen (mit einefachen PollSwitch()-Aufrufen gehen mir Encoderticks verloren und der Asuro fährt unkontrollierte Kurven).
Daher prüfe ich bei der Geradeausfahrt, ob switched gesetzt ist. Ist dies der Fall, so kann ich einfach zurücksetzen, drehen und weiterfahren (nach dem Zurücksetzen setze ich auch switched wieder auf 0 und führe StartSwitch() aus, damit es beim nächsten Mal wieder funktioniert). Soweit so gut - bis dahin läuft alles wie gewünscht.
Kritisch wird es, wenn ich die Drehrichtung anhand der Taster auswählen möchte. Da die Interruptbehandlung nur das switched Flag setzt, muss ich den gedrückten Taster also explizit herausfinden. Dazu nutze ich der Einfachheit halber PollSwitch() - wenn der Asuro ohnehin angehalten hat, ist diese Funktion ja nicht mehr zeitkritisch und ich kann bequem die langsame AD-Konversion ausführen.
Leider tritt hierbei das Problem auf, dass ab diesem Moment permanent Interrupts bei den Schaltern auftreten - sprich: sobald der Asuro fertig gedreht hat, stellt er fest, dass er schon wieder kollidiert ist (auch wenn das nicht der Fall ist).
Ich habe zu diesem Problem schon eine ganze Weile im Datenblatt geblättert, muss aber etwas übersehen haben. Vielleicht hat ja jemand von euch dieselben Schwierigkeiten und einen Tipp parat, woran das liegen könnte (?). Wenn ja, würde ich mich über eine Antwort hier freuen.
Gruß
Ole
P.S.: Zum Besseren Verständnis (und vielleicht selbst ausprobieren), füge ich hier mal meinen Code an.
Code:/******************************************************************************* * File: drive.c * Project: Asuro * * ------------ * Description: * ------------ * Implements a simple drive-behavior. * * Features: - Move straight forward. * - Step back, if we bump against an obstacle. * - Turn left or right to aviod the obstacle. * ----------------------------------------------------------------------------- * * -------- * History: * -------- * Version | Author | Date | Comments * --------+----------------+------------+-------------------------------------- * 0.1 | Jan Ole Berndt | 11.12.2005 | Initial version. * ******************************************************************************/ #include "asuro.h" #define FORWARD 1 #define STEP_BACK 2 #define TURN_LEFT 4 #define TURN_RIGHT 8 #define BASE_SPEED 150 int driveState = FORWARD; int turnDirection = TURN_LEFT; /* * Determines whether we have to stop the current move. */ int Interrupted(void) { return switched; } /* * Moves the robot straight forward. * This function is based on the Go()-function from the extended * asuro-library. */ char Forward(int distance, int speed) { int enc_count = 0; int tot_count = 0; int diff = 0; int l_speed = speed, r_speed = speed; enc_count=abs(distance); Encoder_Set(0,0); // reset encoder MotorSpeed(l_speed,r_speed); MotorDir(FWD,FWD); //As long, as we haven't moved as far as we should and are not forced //to stop: while(tot_count<enc_count && !Interrupted()) { tot_count += encoder[LEFT]; diff = encoder[LEFT] - encoder[RIGHT]; if (diff > 0) { //Left faster than right if ((l_speed > speed) || (r_speed > 244)) l_speed -= 10; else r_speed += 10; } if (diff < 0) { //Right faster than left if ((r_speed > speed) || (l_speed > 244)) r_speed -= 10; else l_speed += 10; } Encoder_Set(0,0); // reset encoder MotorSpeed(l_speed,r_speed); Msleep(1); } //If we are forced to stop: if(Interrupted()){ MotorDir(BREAK,BREAK); Msleep(200); } return 0; } /* * Moves the robot in backward direction. */ char StepBack() { unsigned char switches = PollSwitch(); switches &= PollSwitch(); switches &= PollSwitch(); switches &= PollSwitch(); switches &= PollSwitch(); switches &= PollSwitch(); switches &= PollSwitch(); switches &= PollSwitch(); if(switches & 0x07) turnDirection = TURN_LEFT; else turnDirection = TURN_RIGHT; Encoder_Init(); Go(-50,150); switched = 0; StartSwitch(); return 0; } /* * Turns the robot to the left. */ char TurnLeft() { Turn(-100,BASE_SPEED); return 0; } /* * Turns the robot to the right. */ char TurnRight() { Turn(100,BASE_SPEED); return 0; } /* * Returns the next state of the robot. */ int NextState() { if(driveState & STEP_BACK) return turnDirection; else if(Interrupted()) return STEP_BACK; else return FORWARD; } /* * Main function. * Determines the next state of the robot and calls the corresponding function. */ int main(void) { Init(); driveState = FORWARD; Encoder_Init(); //Start Interrupt-handling for odometry. StartSwitch(); //Start Interrupt-handling for switches. while(1){ driveState = NextState(); switch(driveState){ case FORWARD: Forward(500,BASE_SPEED); break; case STEP_BACK: StepBack(); break; case TURN_LEFT: TurnLeft(); break; case TURN_RIGHT: TurnRight(); break; } } //if any error occures and we leave the loop above, //stop the motors and enter stable state (do nothing). MotorSpeed(0,0); MotorDir(BREAK,BREAK); StopSwitch(); Encoder_Stop(); while(1); return 0; }
Lesezeichen