Öffentliche Projekte

/*
* File Name: 	main.cpp
* Author: 		Aitana Almaguer Aguilar
* Company:		Agroscope
* License:		Copyright 2017, Agroscope
*
* Date:			27.01.2017
* Version:		1.0.0
*
* Description:
*
* 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
*/

/*******************************************************************************
* Includes
*******************************************************************************/
// Niboburger Libraries hinzufügen, ohne setup() und loop()
#include <niboburger/robo.h>
#include "maroon.hpp" 				// Maroon Shield Libraries hinzufügen
#include <stdbool.h>				// Boolean Libraries hinzufügen

/*******************************************************************************
* Constants and global variables
*******************************************************************************/
#define MOTOR_CORRECTION 0.69 		// Geschwindigkeitskorrektur der Motoren
#define threshold_low 10			// Hysterese
#define threshold_high 40

/* State machine */
#define INIT 1						// JEPA: Konstanten GROSS schreiben
#define CHECK_BUTTON 2
#define MOVE_FORWARD 3
#define DETERMINE_DIRECTION 4
#define MOVE_BACKWARD_LEFT 5
#define MOVE_BACKWARD_RIGHT 6
#define READ_SENSORS 7
#define UPDATE_MAROON 8
#define STOP 9

/*******************************************************************************
* Function prototypes
*******************************************************************************/
void move_forward_straight(uint16_t speed);
void move_backward_left(uint16_t speed);
void move_backward_right(uint16_t speed);
void move_backward_straight(uint16_t speed);

int main ()
{
	char key;
	int ledNr;
	int r,l,fll,fl,frr,fr;
	unsigned char state = INIT;						// globale Statusvariable
	uint16_t distance_threshold = threshold_high;	// Hysterese
	bool running = 0;								
/*
* Running Flag
* running = true => Robot is moving
* running = false => Robot is not moving
*/
	while(1) {
		/* State machine */
		switch(state) {

case INIT:
			maroon_setup();				// Schnittstelle konfigurieren
			motpid_init();				// Motorregelung initialisieren
			odometry_init();			// Lichtschranken initialisieren
			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();
			sei();						// Enable interrupts
			state = CHECK_BUTTON;
			break;

case READ_SENSORS:
			/* Status wird nach jedem Ablauf aktualisiert */
			analog_wait_update();

fll=analog_getValueExt(ANALOG_FLL,
			                       2); 		// Sensor FLL einlesen
			fl=analog_getValueExt(ANALOG_FL,
			                      2); 		// Sensor FL einlesen
			frr=analog_getValueExt(ANALOG_FR,
			                       2); 		// Sensor FRR einlesen
			fr=analog_getValueExt(ANALOG_FRR,
			                      2); 		// Sensor FR einlesen
			state = UPDATE_MAROON;
			break;

case CHECK_BUTTON:
			key=key_get_char(); 						// Read keys

if (key == 'A') { 							// Taster 1 gedrückt
				running = 1;							// running = True
				state = READ_SENSORS;
			}

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 = READ_SENSORS;
				}

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:
			l=max(fll,fl); 			// Roboter sieht einen linken Bereich
			r=max(frr,
			      fr); 		    // Roboter sieht einen rechten Bereich

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
			motpid_setSpeed(-15,-25);					// rechts zurückfahren
			distance_threshold = threshold_low;					// Hysterese 10
			state = CHECK_BUTTON;						// CHECK_BUTTON
			break;

case MOVE_BACKWARD_LEFT:
			led_setall(0,0,1,1);						// LEDs 3 & 4 leuchten
			motpid_setSpeed(-25,-15);					// links zurückfahren
			distance_threshold = threshold_low;					// Hysterese 10
			state = CHECK_BUTTON;						// CHECK_BUTTON
			break;

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

case UPDATE_MAROON:
			/*
			 * Read distance sensors and display four bar graphs, indicating
			 * the measured distance to the object
			 */
			maroon_loop();
			state = DETERMINE_DIRECTION;
			break;

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

default:										// default case = 0
			break;
		}
	}
}

/*
* Move forward function
* Beide Motoren fahren vorwärts mit der Geschwindigkeit   
* speed [0 ... +1024]
*/
void move_forward_straight(uint16_t speed)
{
	motpwm_setLeft(speed * MOTOR_CORRECTION);
	motpwm_setRight(speed);
}
/*
* Move backward left function
* Beide Motoren fahren links zurück mit der Geschwindigkeit   
* speed [0 ... +1024]
*/
void move_backward_left(uint16_t speed)
{
	motpwm_setLeft(-0.5 * speed * MOTOR_CORRECTION);
	motpwm_setRight(-speed);
}
/*
* Move backward right function
* Beide Motoren fahren rechts zurück mit der Geschwindigkeit   
* speed [0 ... +1024]
*/
void move_backward_right(uint16_t speed)
{
	motpwm_setLeft(-speed * MOTOR_CORRECTION);
	motpwm_setRight(-0.5 * speed);
}
/*
* Move backward function
* Beide Motoren fahren zurück mit der Geschwindigkeit   
* speed [0 ... +1024]
*/
void move_backward_straight(uint16_t speed)
{
	motpwm_setLeft(-speed * MOTOR_CORRECTION);
	motpwm_setRight(-speed);
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

Statemachine/main.cpp [read only]

Werbung

Online