Revision 41:7484471403aa, committed 2022-05-11
- Comitter:
- pmic
- Date:
- Wed May 11 09:44:25 2022 +0200
- Parent:
- 40:924bdbc33391
- Child:
- 42:d2d2db5974c9
- Commit message:
- Delete StateMachine
Changed in this revision
--- a/StateMachine.cpp Wed May 11 09:42:50 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,179 +0,0 @@
-/*
- * StateMachine.cpp
- * Copyright (c) 2022, ZHAW
- * All rights reserved.
- */
-
-#include <cmath>
-#include "StateMachine.h"
-
-using namespace std;
-
-const float StateMachine::PERIOD = 0.01f; // period of task, given in [s]
-const float StateMachine::DISTANCE_THRESHOLD = 0.4f; // minimum allowed distance to obstacle in [m]
-const float StateMachine::TRANSLATIONAL_VELOCITY = 1.0f; // translational velocity in [m/s]
-const float StateMachine::ROTATIONAL_VELOCITY = 1.0f; // rotational velocity in [rad/s]
-const float StateMachine::VELOCITY_THRESHOLD = 0.01; // velocity threshold before switching off, in [m/s] and [rad/s]
-
-/**
- * Creates and initializes a state machine object.
- */
-StateMachine::StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5) : controller(controller), enableMotorDriver(enableMotorDriver), led0(led0), led1(led1), led2(led2), led3(led3), led4(led4), led5(led5), button(button), irSensor0(irSensor0), irSensor1(irSensor1), irSensor2(irSensor2), irSensor3(irSensor3), irSensor4(irSensor4), irSensor5(irSensor5), thread(osPriorityAboveNormal, STACK_SIZE) {
-
- enableMotorDriver = 0;
- state = ROBOT_OFF;
- buttonNow = button;
- buttonBefore = buttonNow;
-
- // start thread and timer interrupt
-
- thread.start(callback(this, &StateMachine::run));
- ticker.attach(callback(this, &StateMachine::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * PERIOD)});
-}
-
-/**
- * Deletes the state machine object and releases all allocated resources.
- */
-StateMachine::~StateMachine() {
-
- ticker.detach();
-}
-
-/**
- * Gets the actual state of this state machine.
- * @return the actual state as an int constant.
- */
-int StateMachine::getState() {
-
- return state;
-}
-
-/**
- * This method is called by the ticker timer interrupt service routine.
- * It sends a flag to the thread to make it run again.
- */
-void StateMachine::sendThreadFlag() {
-
- thread.flags_set(threadFlag);
-}
-
-/**
- * This is an internal method of the state machine that is running periodically.
- */
-void StateMachine::run() {
-
- int buttonPress;
-
- while (true) {
-
- // wait for the periodic thread flag
-
- ThisThread::flags_wait_any(threadFlag);
-
- // set the leds based on distance measurements
-
- led0 = irSensor0 < DISTANCE_THRESHOLD;
- led1 = irSensor1 < DISTANCE_THRESHOLD;
- led2 = irSensor2 < DISTANCE_THRESHOLD;
- led3 = irSensor3 < DISTANCE_THRESHOLD;
- led4 = irSensor4 < DISTANCE_THRESHOLD;
- led5 = irSensor5 < DISTANCE_THRESHOLD;
-
- // read the button
- buttonNow = button;
- buttonPress = buttonNow > buttonBefore;
- buttonBefore = buttonNow;
-
- // implementation of the state machine
-
- switch (state) {
-
- case ROBOT_OFF:
-
- if (buttonPress) { // detect button rising edge
-
- enableMotorDriver = 1;
-
- controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
- controller.setRotationalVelocity(0.0f);
-
- state = MOVE_FORWARD;
- }
-
- break;
-
- case MOVE_FORWARD:
- if (buttonPress) {
- controller.setTranslationalVelocity(0);
- controller.setRotationalVelocity(0);
- state = SLOWING_DOWN;
- break;
- }
-
- if (irSensor2 < DISTANCE_THRESHOLD && irSensor2 < DISTANCE_THRESHOLD) {
- controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2);
- controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
- state = TURN_RIGHT;
- break;
- } else if (irSensor4 < DISTANCE_THRESHOLD && irSensor4 < DISTANCE_THRESHOLD) {
- controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2);
- controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
- state = TURN_LEFT;
- break;
- } else if (irSensor3 < DISTANCE_THRESHOLD/2) {
- controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY);
- controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
- state = TURN_RIGHT;
- break;
- }
-
- break;
-
- case TURN_LEFT:
- if (buttonPress) {
- controller.setTranslationalVelocity(0);
- controller.setRotationalVelocity(0);
- state = SLOWING_DOWN;
- break;
- }
-
- if ( (irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD) ) {
- controller.setRotationalVelocity(0);
- controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
- state = MOVE_FORWARD;
- break;
- }
- break;
-
- case TURN_RIGHT:
- if (buttonPress) {
- controller.setTranslationalVelocity(0);
- controller.setRotationalVelocity(0);
- state = SLOWING_DOWN;
- break;
- }
-
- if ( (irSensor2 > DISTANCE_THRESHOLD) && (irSensor3 > DISTANCE_THRESHOLD) && (irSensor4 > DISTANCE_THRESHOLD) ) {
- controller.setRotationalVelocity(0);
- controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
- state = MOVE_FORWARD;
- break;
- }
- break;
-
- case SLOWING_DOWN:
- if (abs(controller.getActualTranslationalVelocity()) < VELOCITY_THRESHOLD
- && abs(controller.getActualRotationalVelocity()) > VELOCITY_THRESHOLD) {
- state = ROBOT_OFF;
- enableMotorDriver = 0;
- state = ROBOT_OFF;
- }
-
- break;
-
- default:
-
- state = ROBOT_OFF;
- }
- }
-}
--- a/StateMachine.h Wed May 11 09:42:50 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,71 +0,0 @@
-/*
- * StateMachine.h
- * Copyright (c) 2022, ZHAW
- * All rights reserved.
- */
-
-#ifndef STATE_MACHINE_H_
-#define STATE_MACHINE_H_
-
-#include <cstdlib>
-#include <mbed.h>
-#include "Controller.h"
-#include "IRSensor.h"
-
-/**
- * This class implements a simple state machine for a mobile robot.
- * It allows to move the robot forward, and to turn left or right,
- * depending on distance measurements, to avoid collisions with
- * obstacles.
- */
-class StateMachine {
-
- public:
-
- static const int ROBOT_OFF = 0; // discrete states of this state machine
- static const int MOVE_FORWARD = 1;
- static const int TURN_LEFT = 2;
- static const int TURN_RIGHT = 3;
- static const int SLOWING_DOWN = 4;
-
- StateMachine(Controller& controller, DigitalOut& enableMotorDriver, DigitalOut& led0, DigitalOut& led1, DigitalOut& led2, DigitalOut& led3, DigitalOut& led4, DigitalOut& led5, DigitalIn& button, IRSensor& irSensor0, IRSensor& irSensor1, IRSensor& irSensor2, IRSensor& irSensor3, IRSensor& irSensor4, IRSensor& irSensor5);
- virtual ~StateMachine();
- int getState();
-
- private:
-
- static const unsigned int STACK_SIZE = 4096; // stack size of thread, given in [bytes]
- static const float PERIOD; // period of task, given in [s]
-
- static const float DISTANCE_THRESHOLD; // minimum allowed distance to obstacle in [m]
- static const float TRANSLATIONAL_VELOCITY; // translational velocity in [m/s]
- static const float ROTATIONAL_VELOCITY; // rotational velocity in [rad/s]
- static const float VELOCITY_THRESHOLD; // velocity threshold before switching off, in [m/s] and [rad/s]
-
- Controller& controller;
- DigitalOut& enableMotorDriver;
- DigitalOut& led0;
- DigitalOut& led1;
- DigitalOut& led2;
- DigitalOut& led3;
- DigitalOut& led4;
- DigitalOut& led5;
- DigitalIn& button;
- IRSensor& irSensor0;
- IRSensor& irSensor1;
- IRSensor& irSensor2;
- IRSensor& irSensor3;
- IRSensor& irSensor4;
- IRSensor& irSensor5;
- int state;
- int buttonNow;
- int buttonBefore;
- ThreadFlag threadFlag;
- Thread thread;
- Ticker ticker;
-
- void sendThreadFlag();
- void run();
-};
-
-#endif /* STATE_MACHINE_H_ */
Michael Ernst Peter
