Öffentliche Projekte

/*
 * 	Da ein Motor leicht schneller als der andere läuft, musste die
 *	Geschwindikeit so angepasst werden, dass der Roboter geradeaus fahren kann.
 *	Falls ein Objekt von einem IR-Sensor erfasst wurde und der Schwellwert 1
 *	erreicht ist, soll der Roboter so lange zurückfahren, bis er sich ausserhalb
 *	des Bereichs zwischen Schwellwert 1 und 2 befindet, daher ändert
 *	sich distance_threshold
*/

#include <niboburger/robomain.h> 	// Niboburger Libraries hinzufügen
#include "maroon.hpp" 				// Maroon Shield Libraries hinzufügen
#include <stdbool.h>				// Boolean Libraries hinzufügen

/* Constants */
#define MOTOR_CORRECTION 0.69 		// Geschwindigkeitskorrektur der Motoren

/* State machine */
#define Init 1
#define CHECK_BUTTON 2
#define MOVE_FORWARD 3
#define DETERMINE_DIRECTION 4
#define MOVE_BACKWARD_LEFT 5
#define MOVE_BACKWARD_RIGHT 6
#define STOP 7

/* Move forward function */
void move_forward_straight(uint16_t speed)
{
    motpwm_setLeft(speed * MOTOR_CORRECTION);
    motpwm_setRight(speed);
}

/* Move backward left function */
void move_backward_left(uint16_t speed)
{
    motpwm_setLeft(-0.5 * speed * MOTOR_CORRECTION);
    motpwm_setRight(-speed);
}

/* Move backward right function */
void move_backward_right(uint16_t speed)
{
    motpwm_setLeft(-speed * MOTOR_CORRECTION);
    motpwm_setRight(-0.5 * speed);
}

/* Move backward function */
void move_backward_straight(uint16_t speed)
{
    motpwm_setLeft(-speed * MOTOR_CORRECTION);
    motpwm_setRight(-speed);
}

unsigned char state = Init;						// globale Statusvariable
uint16_t distance_threshold = 40;				// Hysterese
bool running = 0;								// Flag

void setup ()
{
    maroon_setup();				// Schnittstelle konfigurieren
    motpwm_init(); 				// Motoren inisialitieren
    motpwm_setLeft(0); 			// Motor links auf 0 setzen
    motpwm_setRight(0); 		// Motor rechts auf 0 setzen
    delay(100);
    led_init(); 				// LEDs initialisieren
    analog_init(); 				// Analoge Sensoren initialisieren
    maroon_welcome();
}

void loop ()
{
    maroon_loop();
    analog_wait_update(); 		// Status wird nach jedem Ablauf aktualisiert
    char key=key_get_char(); 	// Taster einlesen
    int ledNr;

/* Sensoren einlesen */
    int fll=analog_getValueExt(ANALOG_FLL, 2); 		// Sensor FLL einlesen
    int fl=analog_getValueExt(ANALOG_FL, 2); 		// Sensor FL einlesen
    int frr=analog_getValueExt(ANALOG_FR, 2); 		// Sensor FRR einlesen
    int fr=analog_getValueExt(ANALOG_FRR, 2); 		// Sensor FR einlesen

/* Bereiche definieren */
    int l=max(fll,fl); 			// Roboter sieht einen linken Bereich
    int r=max(frr,fr); 		    // Roboter sieht einen rechten Bereich

/* State machine */
    switch(state) {

case Init:
        state = CHECK_BUTTON;
        break;

case CHECK_BUTTON:
        if (key == 'A') { 							// Taster 1 gedrückt
            running = 1;							// running = True
            state = DETERMINE_DIRECTION;			// DETERMINE_DIRECTION
        }
        if (key == 'B') { 							// Taster 2 gedrückt
            running = 0;							// running = False
            state = STOP;							// STOP
        }
        if( key == '\0') {							// Kein Taster gedrückt
            if(running == 1) {						// Falls running = True
                state = DETERMINE_DIRECTION;		// DETERMINE_DIRECTION
            }
            if(running == 0) {						// Falls running = False
                for (ledNr=1; ledNr<=4; ledNr++) {	// LEDs Lauflicht
                    led_set(ledNr, 1);
                    delay(100);
                    led_set(ledNr, 0);
                    delay(100);
                }
                state = CHECK_BUTTON;				// CHECK_BUTTON
            }
        }
        break;

case DETERMINE_DIRECTION:
        if (l > distance_threshold) {				// l grösser als Hysterese
            state = MOVE_BACKWARD_RIGHT;			// MOVE_BACKWARD_RIGHT
        } else if (r > distance_threshold) {		// r grösser als Hysterese
            state = MOVE_BACKWARD_LEFT;				// MOVE_BACKWARD_LEFT
        } else if ((l < distance_threshold) && (r < distance_threshold)) {
            state = MOVE_FORWARD;			// r & l kleiner als Hysterese
        }											// MOVE_FORWARD
        break;

case MOVE_BACKWARD_RIGHT:
        led_setall(1,1,0,0);						// LEDs 1 & 2 leuchten
        move_backward_right(700);					// rechts zurückfahren
        distance_threshold = 10;					// Hysterese 10
        state = CHECK_BUTTON;						// CHECK_BUTTON
        break;

case MOVE_BACKWARD_LEFT:
        led_setall(0,0,1,1);						// LEDs 3 & 4 leuchten
        move_backward_left(700);					// links zurückfahren
        distance_threshold = 10;					// Hysterese 10
        state = CHECK_BUTTON;						// CHECK_BUTTON
        break;

case MOVE_FORWARD:
        led_setall(0,0,0,0);						// Keine LED leuchtet
        move_forward_straight(1000);				// vorwärts fahren
        distance_threshold = 40;					// Hysterese 40
        state = CHECK_BUTTON;						// CHECK_BUTTON
        break;

case STOP:
        motpwm_setLeft(0);							// Motoren stoppen
        motpwm_setRight(0);
        state = CHECK_BUTTON;						// CHECK_BUTTON
        break;

default:										// default case = 0
        break;

}
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

hindernisse_ausweichen/main.cpp [read only]

Werbung

Online