Michael Ernst Peter
Example project
Dependencies: PM2_Libary Eigen
Revision 37:698d6b73b50c, committed 2022-05-10
- Comitter:
- pmic
- Date:
- Tue May 10 10:04:51 2022 +0200
- Parent:
- 36:23addefb97af
- Child:
- 38:8aae5cbcf25f
- Commit message:
- Included nesserary classes for ROME2 example from bleik
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,192 @@
+/*
+ * Controller.cpp
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#include "Controller.h"
+
+using namespace std;
+
+const float Controller::PERIOD = 0.001f; // period of control task, given in [s]
+const float Controller::M_PI = 3.14159265f; // the mathematical constant PI
+const float Controller::WHEEL_DISTANCE = 0.190f; // distance between wheels, given in [m]
+const float Controller::WHEEL_RADIUS = 0.0375f; // radius of wheels, given in [m]
+const float Controller::MAXIMUM_VELOCITY = 500.0; // maximum wheel velocity, given in [rpm]
+const float Controller::MAXIMUM_ACCELERATION = 1000.0; // maximum wheel acceleration, given in [rpm/s]
+const float Controller::COUNTS_PER_TURN = 1200.0f; // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
+const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s]
+const float Controller::KN = 40.0f; // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
+const float Controller::KP = 0.15f; // speed control parameter
+const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V]
+const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle
+const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle
+
+/**
+ * Creates and initialises the robot controller.
+ * @param pwmLeft a reference to the pwm output for the left motor.
+ * @param pwmRight a reference to the pwm output for the right motor.
+ * @param counterLeft a reference to the encoder counter of the left motor.
+ * @param counterRight a reference to the encoder counter of the right motor.
+ */
+Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounterROME2& counterLeft, EncoderCounterROME2& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight), thread(osPriorityHigh, STACK_SIZE) {
+
+ // initialise pwm outputs
+
+ pwmLeft.period(0.00005f); // pwm period of 50 us
+ pwmLeft = 0.5f; // duty-cycle of 50%
+
+ pwmRight.period(0.00005f); // pwm period of 50 us
+ pwmRight = 0.5f; // duty-cycle of 50%
+
+ // initialise local variables
+
+ translationalVelocity = 0.0f;
+ rotationalVelocity = 0.0f;
+
+ actualTranslationalVelocity = 0.0f;
+ actualRotationalVelocity = 0.0f;
+
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 0.0f;
+
+ actualSpeedLeft = 0.0f;
+ actualSpeedRight = 0.0f;
+
+ motionLeft.setProfileVelocity(MAXIMUM_VELOCITY);
+ motionLeft.setProfileAcceleration(MAXIMUM_ACCELERATION);
+ motionLeft.setProfileDeceleration(MAXIMUM_ACCELERATION);
+
+ motionRight.setProfileVelocity(MAXIMUM_VELOCITY);
+ motionRight.setProfileAcceleration(MAXIMUM_ACCELERATION);
+ motionRight.setProfileDeceleration(MAXIMUM_ACCELERATION);
+
+ previousValueCounterLeft = counterLeft.read();
+ previousValueCounterRight = counterRight.read();
+
+ speedLeftFilter.setPeriod(PERIOD);
+ speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+ speedRightFilter.setPeriod(PERIOD);
+ speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+ // start thread and timer interrupt
+
+ thread.start(callback(this, &Controller::run));
+ ticker.attach(callback(this, &Controller::sendThreadFlag), PERIOD);
+}
+
+/**
+ * Deletes this Controller object.
+ */
+Controller::~Controller() {
+
+ ticker.detach(); // stop the timer interrupt
+}
+
+/**
+ * Sets the desired translational velocity of the robot.
+ * @param velocity the desired translational velocity, given in [m/s].
+ */
+void Controller::setTranslationalVelocity(float velocity) {
+
+ this->translationalVelocity = velocity;
+}
+
+/**
+ * Sets the desired rotational velocity of the robot.
+ * @param velocity the desired rotational velocity, given in [rad/s].
+ */
+void Controller::setRotationalVelocity(float velocity) {
+
+ this->rotationalVelocity = velocity;
+}
+
+/**
+ * Gets the actual translational velocity of the robot.
+ * @return the actual translational velocity, given in [m/s].
+ */
+float Controller::getActualTranslationalVelocity() {
+
+ return actualTranslationalVelocity;
+}
+
+/**
+ * Gets the actual rotational velocity of the robot.
+ * @return the actual rotational velocity, given in [rad/s].
+ */
+float Controller::getActualRotationalVelocity() {
+
+ return actualRotationalVelocity;
+}
+
+/**
+ * This method is called by the ticker timer interrupt service routine.
+ * It sends a flag to the thread to make it run again.
+ */
+void Controller::sendThreadFlag() {
+
+ thread.flags_set(threadFlag);
+}
+
+/**
+ * This is an internal method of the controller that is running periodically.
+ */
+void Controller::run() {
+
+ while (true) {
+
+ // wait for the periodic thread flag
+
+ ThisThread::flags_wait_any(threadFlag);
+
+ // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
+
+ desiredSpeedLeft = (translationalVelocity-WHEEL_DISTANCE/2.0f*rotationalVelocity)/WHEEL_RADIUS*60.0f/2.0f/M_PI;
+ desiredSpeedRight = -(translationalVelocity+WHEEL_DISTANCE/2.0f*rotationalVelocity)/WHEEL_RADIUS*60.0f/2.0f/M_PI;
+
+ // calculate planned speedLeft and speedRight values using the motion planner
+
+ motionLeft.incrementToVelocity(desiredSpeedLeft, PERIOD);
+ motionRight.incrementToVelocity(desiredSpeedRight, PERIOD);
+
+ desiredSpeedLeft = motionLeft.getVelocity();
+ desiredSpeedRight = motionRight.getVelocity();
+
+ // calculate the actual speed of the motors in [rpm]
+
+ short valueCounterLeft = counterLeft.read();
+ short valueCounterRight = counterRight.read();
+
+ short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
+ short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
+
+ previousValueCounterLeft = valueCounterLeft;
+ previousValueCounterRight = valueCounterRight;
+
+ actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
+ actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
+
+ // calculate desired motor voltages Uout
+
+ float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+ float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
+
+ // calculate, limit and set the duty-cycle
+
+ float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
+ if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
+ else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
+ pwmLeft = dutyCycleLeft;
+
+ float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
+ if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
+ else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
+ pwmRight = dutyCycleRight;
+
+ // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
+
+ actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*M_PI/60.0f*WHEEL_RADIUS/2.0f;
+ actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*M_PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,76 @@
+/*
+ * Controller.h
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef CONTROLLER_H_
+#define CONTROLLER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+#include "EncoderCounterROME2.h"
+#include "Motion.h"
+#include "LowpassFilter.h"
+#include "ThreadFlag.h"
+
+/**
+ * This class implements a controller that regulates the
+ * speed of the two motors of the ROME2 mobile robot.
+ */
+class Controller {
+
+ public:
+
+ Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounterROME2& counterLeft, EncoderCounterROME2& counterRight);
+ virtual ~Controller();
+ void setTranslationalVelocity(float velocity);
+ void setRotationalVelocity(float velocity);
+ float getActualTranslationalVelocity();
+ float getActualRotationalVelocity();
+
+ private:
+
+ static const unsigned int STACK_SIZE = 4096; // stack size of thread, given in [bytes]
+ static const float PERIOD; // period of control task, given in [s]
+
+ static const float M_PI; // the mathematical constant PI
+ static const float WHEEL_DISTANCE; // distance between wheels, given in [m]
+ static const float WHEEL_RADIUS; // radius of wheels, given in [m]
+ static const float MAXIMUM_VELOCITY; // maximum wheel velocity, given in [rpm]
+ static const float MAXIMUM_ACCELERATION; // maximum wheel acceleration, given in [rpm/s]
+ static const float COUNTS_PER_TURN;
+ static const float LOWPASS_FILTER_FREQUENCY;
+ static const float KN;
+ static const float KP;
+ static const float MAX_VOLTAGE;
+ static const float MIN_DUTY_CYCLE;
+ static const float MAX_DUTY_CYCLE;
+
+ PwmOut& pwmLeft;
+ PwmOut& pwmRight;
+ EncoderCounterROME2& counterLeft;
+ EncoderCounterROME2& counterRight;
+ float translationalVelocity;
+ float rotationalVelocity;
+ float actualTranslationalVelocity;
+ float actualRotationalVelocity;
+ float desiredSpeedLeft;
+ float desiredSpeedRight;
+ float actualSpeedLeft;
+ float actualSpeedRight;
+ Motion motionLeft;
+ Motion motionRight;
+ short previousValueCounterLeft;
+ short previousValueCounterRight;
+ LowpassFilter speedLeftFilter;
+ LowpassFilter speedRightFilter;
+ ThreadFlag threadFlag;
+ Thread thread;
+ Ticker ticker;
+
+ void sendThreadFlag();
+ void run();
+};
+
+#endif /* CONTROLLER_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounterROME2.cpp Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,185 @@
+/*
+ * EncoderCounterROME2.cpp
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounterROME2.h"
+
+using namespace std;
+
+/**
+ * Creates and initialises the driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ * @param a the input pin for the channel A.
+ * @param b the input pin for the channel B.
+ */
+EncoderCounterROME2::EncoderCounterROME2(PinName a, PinName b) {
+
+ // check pins
+
+ if ((a == PA_15) && (b == PB_3)) {
+
+ // pinmap OK for TIM2 CH1 and CH2
+
+ TIM = TIM2;
+
+ // configure reset and clock control registers
+
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // manually enable port B (port A enabled by mbed library)
+
+ // configure general purpose I/O registers
+
+ GPIOA->MODER &= ~GPIO_MODER_MODER15; // reset port A15
+ GPIOA->MODER |= GPIO_MODER_MODER15_1; // set alternate mode of port A15
+ GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR15; // reset pull-up/pull-down on port A15
+ GPIOA->PUPDR |= GPIO_PUPDR_PUPDR15_1; // set input as pull-down
+ GPIOA->AFR[1] &= ~0xF0000000; // reset alternate function of port A15
+ GPIOA->AFR[1] |= 1 << 4*7; // set alternate funtion 1 of port A15
+
+ GPIOB->MODER &= ~GPIO_MODER_MODER3; // reset port B3
+ GPIOB->MODER |= GPIO_MODER_MODER3_1; // set alternate mode of port B3
+ GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR3; // reset pull-up/pull-down on port B3
+ GPIOB->PUPDR |= GPIO_PUPDR_PUPDR3_1; // set input as pull-down
+ GPIOB->AFR[0] &= ~(0xF << 4*3); // reset alternate function of port B3
+ GPIOB->AFR[0] |= 1 << 4*3; // set alternate funtion 1 of port B3
+
+ // configure reset and clock control registers
+
+ RCC->APB1RSTR |= RCC_APB1RSTR_TIM2RST; //reset TIM2 controller
+ RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM2RST;
+
+ RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // TIM2 clock enable
+
+ } else if ((a == PB_4) && (b == PC_7)) {
+
+ // pinmap OK for TIM3 CH1 and CH2
+
+ TIM = TIM3;
+
+ // configure reset and clock control registers
+
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // manually enable port B
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // manually enable port C
+
+ // configure general purpose I/O registers
+
+ GPIOB->MODER &= ~GPIO_MODER_MODER4; // reset port B4
+ GPIOB->MODER |= GPIO_MODER_MODER4_1; // set alternate mode of port B4
+ GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR4; // reset pull-up/pull-down on port B4
+ GPIOB->PUPDR |= GPIO_PUPDR_PUPDR4_1; // set input as pull-down
+ GPIOB->AFR[0] &= ~(0xF << 4*4); // reset alternate function of port B4
+ GPIOB->AFR[0] |= 2 << 4*4; // set alternate funtion 2 of port B4
+
+ GPIOC->MODER &= ~GPIO_MODER_MODER7; // reset port C7
+ GPIOC->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port C7
+ GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port C7
+ GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down
+ GPIOC->AFR[0] &= ~0xF0000000; // reset alternate function of port C7
+ GPIOC->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port C7
+
+ // configure reset and clock control registers
+
+ RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST; //reset TIM3 controller
+ RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+
+ RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // TIM3 clock enable
+
+ } else if ((a == PD_12) && (b == PD_13)) {
+
+ // pinmap OK for TIM4 CH1 and CH2
+
+ TIM = TIM4;
+
+ // configure reset and clock control registers
+
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN; // manually enable port D
+
+ // configure general purpose I/O registers
+
+ GPIOD->MODER &= ~GPIO_MODER_MODER12; // reset port D12
+ GPIOD->MODER |= GPIO_MODER_MODER12_1; // set alternate mode of port D12
+ GPIOD->PUPDR &= ~GPIO_PUPDR_PUPDR12; // reset pull-up/pull-down on port D12
+ GPIOD->PUPDR |= GPIO_PUPDR_PUPDR12_1; // set input as pull-down
+ GPIOD->AFR[1] &= ~(0xF << 4*4); // reset alternate function of port D12
+ GPIOD->AFR[1] |= 2 << 4*4; // set alternate funtion 2 of port D12
+
+ GPIOD->MODER &= ~GPIO_MODER_MODER13; // reset port D13
+ GPIOD->MODER |= GPIO_MODER_MODER13_1; // set alternate mode of port D13
+ GPIOD->PUPDR &= ~GPIO_PUPDR_PUPDR13; // reset pull-up/pull-down on port D13
+ GPIOD->PUPDR |= GPIO_PUPDR_PUPDR13_1; // set input as pull-down
+ GPIOD->AFR[1] &= ~(0xF << 4*5); // reset alternate function of port D13
+ GPIOD->AFR[1] |= 2 << 4*5; // set alternate funtion 2 of port D13
+
+ // configure reset and clock control registers
+
+ RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST; //reset TIM4 controller
+ RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST;
+
+ RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // TIM4 clock enable
+
+ } else {
+
+ printf("pinmap not found for peripheral\n");
+
+ TIM = NULL;
+ }
+
+ // disable deep sleep for timer clocks
+
+ sleep_manager_lock_deep_sleep();
+
+ // configure general purpose timer 2, 3 or 4
+
+ if (TIM != NULL) {
+
+ TIM->CR1 = 0x0000; // counter disable
+ TIM->CR2 = 0x0000; // reset master mode selection
+ TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
+ TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+ TIM->CCMR2 = 0x0000; // reset capture mode register 2
+ TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+ TIM->CNT = 0x0000; // reset counter value
+ TIM->ARR = 0xFFFF; // auto reload register
+ TIM->CR1 = TIM_CR1_CEN; // counter enable
+ }
+}
+
+/**
+ * Deletes this EncoderCounterROME2 object.
+ */
+EncoderCounterROME2::~EncoderCounterROME2() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounterROME2::reset() {
+
+ TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounterROME2::reset(short offset) {
+
+ TIM->CNT = -offset;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+short EncoderCounterROME2::read() {
+
+ return (short)(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounterROME2::operator short() {
+
+ return read();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounterROME2.h Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,33 @@
+/*
+ * EncoderCounterROME2.h
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_ROME2_H_
+#define ENCODER_COUNTER_ROME2_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounterROME2 {
+
+ public:
+
+ EncoderCounterROME2(PinName a, PinName b);
+ virtual ~EncoderCounterROME2();
+ void reset();
+ void reset(short offset);
+ short read();
+ operator short();
+
+ private:
+
+ TIM_TypeDef* TIM;
+};
+
+#endif /* ENCODER_COUNTER_ROME2_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensor.cpp Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,51 @@
+/*
+ * IRSensor.h
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "IRSensor.h"
+
+using namespace std;
+
+/**
+ * Creates and initialises the driver to read the distance sensors.
+ * @param distance the analog input to read a distance value from.
+ * @param bit0 a digital output to control the multiplexer.
+ * @param bit1 a digital output to control the multiplexer.
+ * @param bit2 a digital output to control the multiplexer.
+ * @param number the number of the sensor. This value must be between 0 and 5.
+ */
+IRSensor::IRSensor(AnalogIn& distance, DigitalOut& bit0, DigitalOut& bit1, DigitalOut& bit2, int number) : distance(distance), bit0(bit0), bit1(bit1), bit2(bit2) {
+
+ this->number = number;
+}
+
+/**
+ * Deletes this IRSensor object and releases all allocated resources.
+ */
+IRSensor::~IRSensor() {}
+
+/**
+ * This method reads from the distance sensor.
+ * @return a distance value, given in [m].
+ */
+float IRSensor::read() {
+
+ bit0 = (number >> 0) & 1;
+ bit1 = (number >> 1) & 1;
+ bit2 = (number >> 2) & 1;
+
+ float d = 0.09f/(distance+0.001f)-0.03f; // calculate the distance in [m]
+
+ return d;
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+IRSensor::operator float() {
+
+ return read();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensor.h Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,35 @@
+/*
+ * IRSensor.h
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef IR_SENSOR_H_
+#define IR_SENSOR_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the distance sensors
+ * of the ROME2 mobile robot.
+ */
+class IRSensor {
+
+ public:
+
+ IRSensor(AnalogIn& distance, DigitalOut& bit0, DigitalOut& bit1, DigitalOut& bit2, int number);
+ virtual ~IRSensor();
+ float read();
+ operator float();
+
+ private:
+
+ AnalogIn& distance;
+ DigitalOut& bit0;
+ DigitalOut& bit1;
+ DigitalOut& bit2;
+ int number;
+};
+
+#endif /* IR_SENSOR_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.cpp Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,601 @@
+/*
+ * Motion.cpp
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include <algorithm>
+#include "Motion.h"
+
+using namespace std;
+
+const float Motion::DEFAULT_LIMIT = 1.0f; // default value for limits
+const float Motion::MINIMUM_LIMIT = 1.0e-9f; // smallest value allowed for limits
+
+/**
+ * Creates a <code>Motion</code> object.
+ * The values for position, velocity and acceleration are set to 0.
+ */
+Motion::Motion() {
+
+ position = 0.0;
+ velocity = 0.0f;
+
+ profileVelocity = DEFAULT_LIMIT;
+ profileAcceleration = DEFAULT_LIMIT;
+ profileDeceleration = DEFAULT_LIMIT;
+}
+
+/**
+ * Creates a <code>Motion</code> object with given values for position and velocity.
+ * @param position the initial position value of this motion, given in [m] or [rad].
+ * @param velocity the initial velocity value of this motion, given in [m/s] or [rad/s].
+ */
+Motion::Motion(double position, float velocity) {
+
+ this->position = position;
+ this->velocity = velocity;
+
+ profileVelocity = DEFAULT_LIMIT;
+ profileAcceleration = DEFAULT_LIMIT;
+ profileDeceleration = DEFAULT_LIMIT;
+}
+
+/**
+ * Creates a <code>Motion</code> object with given values for position and velocity.
+ * @param motion another <code>Motion</code> object to copy the values from.
+ */
+Motion::Motion(const Motion& motion) {
+
+ position = motion.position;
+ velocity = motion.velocity;
+
+ profileVelocity = motion.profileVelocity;
+ profileAcceleration = motion.profileAcceleration;
+ profileDeceleration = motion.profileDeceleration;
+}
+
+/**
+ * Deletes the Motion object.
+ */
+Motion::~Motion() {}
+
+/**
+ * Sets the values for position and velocity.
+ * @param position the desired position value of this motion, given in [m] or [rad].
+ * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
+ */
+void Motion::set(double position, float velocity) {
+
+ this->position = position;
+ this->velocity = velocity;
+}
+
+/**
+ * Sets the values for position and velocity.
+ * @param motion another <code>Motion</code> object to copy the values from.
+ */
+void Motion::set(const Motion& motion) {
+
+ position = motion.position;
+ velocity = motion.velocity;
+}
+
+/**
+ * Sets the position value.
+ * @param position the desired position value of this motion, given in [m] or [rad].
+ */
+void Motion::setPosition(double position) {
+
+ this->position = position;
+}
+
+/**
+ * Gets the position value.
+ * @return the position value of this motion, given in [m] or [rad].
+ */
+double Motion::getPosition() {
+
+ return position;
+}
+
+/**
+ * Sets the velocity value.
+ * @param velocity the desired velocity value of this motion, given in [m/s] or [rad/s].
+ */
+void Motion::setVelocity(float velocity) {
+
+ this->velocity = velocity;
+}
+
+/**
+ * Gets the velocity value.
+ * @return the velocity value of this motion, given in [m/s] or [rad/s].
+ */
+float Motion::getVelocity() {
+
+ return velocity;
+}
+
+/**
+ * Sets the limit for the velocity value.
+ * @param profileVelocity the limit of the velocity.
+ */
+void Motion::setProfileVelocity(float profileVelocity) {
+
+ if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
+}
+
+/**
+ * Sets the limit for the acceleration value.
+ * @param profileAcceleration the limit of the acceleration.
+ */
+void Motion::setProfileAcceleration(float profileAcceleration) {
+
+ if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
+}
+
+/**
+ * Sets the limit for the deceleration value.
+ * @param profileDeceleration the limit of the deceleration.
+ */
+void Motion::setProfileDeceleration(float profileDeceleration) {
+
+ if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
+}
+
+/**
+ * Sets the limits for velocity, acceleration and deceleration values.
+ * @param profileVelocity the limit of the velocity.
+ * @param profileAcceleration the limit of the acceleration.
+ * @param profileDeceleration the limit of the deceleration.
+ */
+void Motion::setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration) {
+
+ if (profileVelocity > MINIMUM_LIMIT) this->profileVelocity = profileVelocity; else this->profileVelocity = MINIMUM_LIMIT;
+ if (profileAcceleration > MINIMUM_LIMIT) this->profileAcceleration = profileAcceleration; else this->profileAcceleration = MINIMUM_LIMIT;
+ if (profileDeceleration > MINIMUM_LIMIT) this->profileDeceleration = profileDeceleration; else this->profileDeceleration = MINIMUM_LIMIT;
+}
+
+/**
+ * Gets the time needed to move to a given target position.
+ * @param targetPosition the desired target position given in [m] or [rad].
+ * @return the time to move to the target position, given in [s].
+ */
+float Motion::getTimeToPosition(double targetPosition) {
+
+ // calculate position, when velocity is reduced to zero
+
+ double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
+
+ if (targetPosition > stopPosition) { // positive velocity required
+
+ if (velocity > profileVelocity) { // slow down to profile velocity first
+
+ float t1 = (velocity-profileVelocity)/profileDeceleration;
+ float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ return t1+t2+t3;
+
+ } else if (velocity > 0.0f) { // speed up to profile velocity
+
+ float t1 = (profileVelocity-velocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+ t1 = (maxVelocity-velocity)/profileAcceleration;
+ t2 = 0.0f;
+ t3 = maxVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3;
+
+ } else { // slow down to zero first, and then speed up to profile velocity
+
+ float t1 = -velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+ t2 = maxVelocity/profileAcceleration;
+ t3 = 0.0f;
+ t4 = maxVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3+t4;
+ }
+
+ } else { // negative velocity required
+
+ if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
+
+ float t1 = (-profileVelocity-velocity)/profileDeceleration;
+ float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ return t1+t2+t3;
+
+ } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
+
+ float t1 = (velocity+profileVelocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+ t1 = (velocity-minVelocity)/profileAcceleration;
+ t2 = 0.0f;
+ t3 = -minVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3;
+
+ } else { // slow down to zero first, and then speed up to (negative) profile velocity
+
+ float t1 = velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+ t2 = -minVelocity/profileAcceleration;
+ t3 = 0.0f;
+ t4 = -minVelocity/profileDeceleration;
+ }
+
+ return t1+t2+t3+t4;
+ }
+ }
+}
+
+/**
+ * Increments the current motion towards a given target velocity.
+ * @param targetVelocity the desired target velocity given in [m/s] or [rad/s].
+ * @param period the time period to increment the motion values for, given in [s].
+ */
+void Motion::incrementToVelocity(float targetVelocity, float period) {
+
+ if (targetVelocity < -profileVelocity) targetVelocity = -profileVelocity;
+ else if (targetVelocity > profileVelocity) targetVelocity = profileVelocity;
+
+ if (targetVelocity > 0.0f) {
+
+ if (velocity > targetVelocity) { // slow down to target velocity
+
+ float t1 = (velocity-targetVelocity)/profileDeceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ } else {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)(velocity*(period-t1));
+ }
+
+ } else if (velocity > 0.0f) { // speed up to target velocity
+
+ float t1 = (targetVelocity-velocity)/profileAcceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity+profileAcceleration*0.5f*period)*period);
+ velocity += profileAcceleration*period;
+ } else {
+ position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += (double)(velocity*(period-t1));
+ }
+
+ } else { // slow down to zero first, and then speed up to target velocity
+
+ float t1 = -velocity/profileDeceleration;
+ float t2 = targetVelocity/profileAcceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += profileAcceleration*(period-t1);
+ } else {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += (double)(velocity*(period-t1-t2));
+ }
+ }
+
+ } else {
+
+ if (velocity < targetVelocity) { // slow down to (negative) target velocity
+
+ float t1 = (targetVelocity-velocity)/profileDeceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ } else {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)(velocity*(period-t1));
+ }
+
+ } else if (velocity < 0.0f) { // speed up to (negative) target velocity
+
+ float t1 = (velocity-targetVelocity)/profileAcceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity-profileAcceleration*0.5f*period)*period);
+ velocity += -profileAcceleration*period;
+ } else {
+ position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += (double)(velocity*(period-t1));
+ }
+
+ } else { // slow down to zero first, and then speed up to (negative) target velocity
+
+ float t1 = velocity/profileDeceleration;
+ float t2 = -targetVelocity/profileAcceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += -profileAcceleration*(period-t1);
+ } else {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += (double)(velocity*(period-t1-t2));
+ }
+ }
+ }
+}
+
+/**
+ * Increments the current motion towards a given target position.
+ * @param targetPosition the desired target position given in [m] or [rad].
+ * @param period the time period to increment the motion values for, given in [s].
+ */
+void Motion::incrementToPosition(double targetPosition, float period) {
+
+ // calculate position, when velocity is reduced to zero
+
+ double stopPosition = (velocity > 0.0f) ? position+(double)(velocity*velocity/profileDeceleration*0.5f) : position-(double)(velocity*velocity/profileDeceleration*0.5f);
+
+ if (targetPosition > stopPosition) { // positive velocity required
+
+ if (velocity > profileVelocity) { // slow down to profile velocity first
+
+ float t1 = (velocity-profileVelocity)/profileDeceleration;
+ float t2 = (float)(targetPosition-stopPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)(velocity*(period-t1));
+ } else if (t1+t2+t3 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += -profileDeceleration*(period-t1-t2);
+ } else {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
+ velocity += -profileDeceleration*t3;
+ }
+
+ } else if (velocity > 0.0f) { // speed up to profile velocity
+
+ float t1 = (profileVelocity-velocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = ((float)(targetPosition-position)-(velocity+profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+ t1 = (maxVelocity-velocity)/profileAcceleration;
+ t2 = 0.0f;
+ t3 = maxVelocity/profileDeceleration;
+ }
+
+ if (t1 > period) {
+ position += (double)((velocity+profileAcceleration*0.5f*period)*period);
+ velocity += profileAcceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += (double)(velocity*(period-t1));
+ } else if (t1+t2+t3 > period) {
+ position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += -profileDeceleration*(period-t1-t2);
+ } else {
+ position += (double)((velocity+profileAcceleration*0.5f*t1)*t1);
+ velocity += profileAcceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity-profileDeceleration*0.5f*t3)*t3);
+ velocity += -profileDeceleration*t3;
+ }
+
+ } else { // slow down to zero first, and then speed up to profile velocity
+
+ float t1 = -velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = ((float)(targetPosition-position)-velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float maxVelocity = sqrt((2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+ t2 = maxVelocity/profileAcceleration;
+ t3 = 0.0f;
+ t4 = maxVelocity/profileDeceleration;
+ }
+
+ if (t1 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)((velocity+profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += profileAcceleration*(period-t1);
+ } else if (t1+t2+t3 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += (double)(velocity*(period-t1-t2));
+ } else if (t1+t2+t3+t4 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += (double)(velocity*t3);
+ position += (double)((velocity-profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
+ velocity += -profileDeceleration*(period-t1-t2-t3);
+ } else {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)((velocity+profileAcceleration*0.5f*t2)*t2);
+ velocity += profileAcceleration*t2;
+ position += (double)(velocity*t3);
+ position += (double)((velocity-profileDeceleration*0.5f*t4)*t4);
+ velocity += -profileDeceleration*t4;
+ }
+ }
+
+ } else { // negative velocity required
+
+ if (velocity < -profileVelocity) { // slow down to (negative) profile velocity first
+
+ float t1 = (-profileVelocity-velocity)/profileDeceleration;
+ float t2 = (float)(stopPosition-targetPosition)/profileVelocity;
+ float t3 = profileVelocity/profileDeceleration;
+
+ if (t1 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*period)*period);
+ velocity += profileDeceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)(velocity*(period-t1));
+ } else if (t1+t2+t3 > period) {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += profileDeceleration*(period-t1-t2);
+ } else {
+ position += (double)((velocity+profileDeceleration*0.5f*t1)*t1);
+ velocity += profileDeceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
+ velocity += profileDeceleration*t3;
+ }
+
+ } else if (velocity < 0.0f) { // speed up to (negative) profile velocity
+
+ float t1 = (velocity+profileVelocity)/profileAcceleration;
+ float t3 = profileVelocity/profileDeceleration;
+ float t2 = ((float)(position-targetPosition)+(velocity-profileVelocity)*0.5f*t1)/profileVelocity-0.5f*t3;
+
+ if (t2 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileAcceleration+velocity*velocity)*profileDeceleration/(profileAcceleration+profileDeceleration));
+ t1 = (velocity-minVelocity)/profileAcceleration;
+ t2 = 0.0f;
+ t3 = -minVelocity/profileDeceleration;
+ }
+
+ if (t1 > period) {
+ position += (double)((velocity-profileAcceleration*0.5f*period)*period);
+ velocity += -profileAcceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += (double)(velocity*(period-t1));
+ } else if (t1+t2+t3 > period) {
+ position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2))*(period-t1-t2));
+ velocity += profileDeceleration*(period-t1-t2);
+ } else {
+ position += (double)((velocity-profileAcceleration*0.5f*t1)*t1);
+ velocity += -profileAcceleration*t1;
+ position += (double)(velocity*t2);
+ position += (double)((velocity+profileDeceleration*0.5f*t3)*t3);
+ velocity += profileDeceleration*t3;
+ }
+
+ } else { // slow down to zero first, and then speed up to (negative) profile velocity
+
+ float t1 = velocity/profileDeceleration;
+ float t2 = profileVelocity/profileAcceleration;
+ float t4 = profileVelocity/profileDeceleration;
+ float t3 = (-(float)(targetPosition-position)+velocity*0.5f*t1)/profileVelocity-0.5f*(t2+t4);
+
+ if (t3 < 0.0f) {
+ float minVelocity = -sqrt((-2.0f*(float)(targetPosition-position)*profileDeceleration+velocity*velocity)*profileAcceleration/(profileAcceleration+profileDeceleration));
+ t2 = -minVelocity/profileAcceleration;
+ t3 = 0.0f;
+ t4 = -minVelocity/profileDeceleration;
+ }
+
+ if (t1 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*period)*period);
+ velocity += -profileDeceleration*period;
+ } else if (t1+t2 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)((velocity-profileAcceleration*0.5f*(period-t1))*(period-t1));
+ velocity += -profileAcceleration*(period-t1);
+ } else if (t1+t2+t3 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += (double)(velocity*(period-t1-t2));
+ } else if (t1+t2+t3+t4 > period) {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += (double)(velocity*t3);
+ position += (double)((velocity+profileDeceleration*0.5f*(period-t1-t2-t3))*(period-t1-t2-t3));
+ velocity += profileDeceleration*(period-t1-t2-t3);
+ } else {
+ position += (double)((velocity-profileDeceleration*0.5f*t1)*t1);
+ velocity += -profileDeceleration*t1;
+ position += (double)((velocity-profileAcceleration*0.5f*t2)*t2);
+ velocity += -profileAcceleration*t2;
+ position += (double)(velocity*t3);
+ position += (double)((velocity+profileDeceleration*0.5f*t4)*t4);
+ velocity += profileDeceleration*t4;
+ }
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.h Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,59 @@
+/*
+ * Motion.h
+ * Copyright (c) 2022, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef MOTION_H_
+#define MOTION_H_
+
+#include <cstdlib>
+
+/**
+ * This class keeps the motion values <code>position</code> and <code>velocity</code>, and
+ * offers methods to increment these values towards a desired target position or velocity.
+ * <br/>
+ * To increment the current motion values, this class uses a simple 2nd order motion planner.
+ * This planner calculates the motion to the target position or velocity with the various motion
+ * phases, based on given limits for the profile velocity, acceleration and deceleration.
+ * <br/>
+ * Note that the trajectory is calculated every time the motion state is incremented.
+ * This allows to change the target position or velocity, as well as the limits for profile
+ * velocity, acceleration and deceleration at any time.
+ */
+class Motion {
+
+ public:
+
+ double position; /**< The position value of this motion, given in [m] or [rad]. */
+ float velocity; /**< The velocity value of this motion, given in [m/s] or [rad/s]. */
+
+ Motion();
+ Motion(double position, float velocity);
+ Motion(const Motion& motion);
+ virtual ~Motion();
+ void set(double position, float velocity);
+ void set(const Motion& motion);
+ void setPosition(double position);
+ double getPosition();
+ void setVelocity(float velocity);
+ float getVelocity();
+ void setProfileVelocity(float profileVelocity);
+ void setProfileAcceleration(float profileAcceleration);
+ void setProfileDeceleration(float profileDeceleration);
+ void setLimits(float profileVelocity, float profileAcceleration, float profileDeceleration);
+ float getTimeToPosition(double targetPosition);
+ void incrementToVelocity(float targetVelocity, float period);
+ void incrementToPosition(double targetPosition, float period);
+
+ private:
+
+ static const float DEFAULT_LIMIT; // default value for limits
+ static const float MINIMUM_LIMIT; // smallest value allowed for limits
+
+ float profileVelocity;
+ float profileAcceleration;
+ float profileDeceleration;
+};
+
+#endif /* MOTION_H_ */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.cpp Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,179 @@
+/*
+ * 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), 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;
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.h Tue May 10 10:04:51 2022 +0200
@@ -0,0 +1,71 @@
+/*
+ * 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_ */
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed OS |
| Created: | 10 May 2022 |
| Imports: | 2 |
| Forks: | 0 |
| Commits: | 53 |
| Dependents: | 0 |
| Dependencies: | 2 |
| Followers: | 3 |
