ROME2
ROME_P5
Dependencies: mbed
Revision 0:29be10cb0afc, committed 2018-04-27
- Comitter:
- Inaueadr
- Date:
- Fri Apr 27 08:47:34 2018 +0000
- Commit message:
- Hallo
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,244 @@
+/*
+ * Controller.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "Controller.h"
+
+using namespace std;
+
+const float Controller::PERIOD = 0.001f; // period of control task, given in [s]
+const float Controller::PI = 3.14159265f; // the constant PI
+const float Controller::WHEEL_DISTANCE = 0.170f; // distance between wheels, given in [m]
+const float Controller::WHEEL_RADIUS = 0.0375f; // radius of wheels, given in [m]
+const float Controller::COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
+const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
+const float Controller::KN = 40.0f; // speed constant of motor, given in [rpm/V]
+const float Controller::KP = 0.2f; // speed controller gain, given in [V/rpm]
+const float Controller::MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
+const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
+const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
+
+/**
+ * Creates and initializes a Controller object.
+ * @param pwmLeft a pwm output object to set the duty cycle for the left motor.
+ * @param pwmRight a pwm output object to set the duty cycle for the right motor.
+ * @param counterLeft an encoder counter object to read the position of the left motor.
+ * @param counterRight an encoder counter object to read the position of the right motor.
+ */
+Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
+
+ // initialize periphery drivers
+
+ pwmLeft.period(0.00005f);
+ pwmLeft.write(0.5f);
+
+ pwmRight.period(0.00005f);
+ pwmRight.write(0.5f);
+
+ // initialize local variables
+
+ translationalMotion.setProfileVelocity(1.5f);
+ translationalMotion.setProfileAcceleration(1.5f);
+ translationalMotion.setProfileDeceleration(3.0f);
+
+ rotationalMotion.setProfileVelocity(3.0f);
+ rotationalMotion.setProfileAcceleration(15.0f);
+ rotationalMotion.setProfileDeceleration(15.0f);
+
+ translationalVelocity = 0.0f;
+ rotationalVelocity = 0.0f;
+ actualTranslationalVelocity = 0.0f;
+ actualRotationalVelocity = 0.0f;
+
+ previousValueCounterLeft = counterLeft.read();
+ previousValueCounterRight = counterRight.read();
+
+ speedLeftFilter.setPeriod(PERIOD);
+ speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+ speedRightFilter.setPeriod(PERIOD);
+ speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 0.0f;
+
+ actualSpeedLeft = 0.0f;
+ actualSpeedRight = 0.0f;
+
+ x = 0.0f;
+ y = 0.0f;
+ alpha = 0.0f;
+
+ // start periodic task
+
+ ticker.attach(callback(this, &Controller::run), PERIOD);
+}
+
+/**
+ * Deletes the Controller object and releases all allocated resources.
+ */
+Controller::~Controller() {
+
+ ticker.detach();
+}
+
+/**
+ * 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;
+}
+
+/**
+ * Sets the actual x coordinate of the robots position.
+ * @param x the x coordinate of the position, given in [m].
+ */
+void Controller::setX(float x) {
+
+ this->x = x;
+}
+
+/**
+ * Gets the actual x coordinate of the robots position.
+ * @return the x coordinate of the position, given in [m].
+ */
+float Controller::getX() {
+
+ return x;
+}
+
+/**
+ * Sets the actual y coordinate of the robots position.
+ * @param y the y coordinate of the position, given in [m].
+ */
+void Controller::setY(float y) {
+
+ this->y = y;
+}
+
+/**
+ * Gets the actual y coordinate of the robots position.
+ * @return the y coordinate of the position, given in [m].
+ */
+float Controller::getY() {
+
+ return y;
+}
+
+/**
+ * Sets the actual orientation of the robot.
+ * @param alpha the orientation, given in [rad].
+ */
+void Controller::setAlpha(float alpha) {
+
+ this->alpha = alpha;
+}
+
+/**
+ * Gets the actual orientation of the robot.
+ * @return the orientation, given in [rad].
+ */
+float Controller::getAlpha() {
+
+ return alpha;
+}
+
+/**
+ * This method is called periodically by the ticker object and contains the
+ * algorithm of the speed controller.
+ */
+void Controller::run() {
+
+ // calculate the planned velocities using the motion planner
+
+ translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
+ rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
+
+ // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
+
+ desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
+ desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
+
+ // calculate actual speed of 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 motor phase voltages
+
+ float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+ float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
+
+ // calculate, limit and set duty cycles
+
+ 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.write(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.write(dutyCycleRight);
+
+ // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
+
+ actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
+ actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
+
+ // calculate the actual robot pose
+
+ float deltaTranslation = actualTranslationalVelocity*PERIOD;
+ float deltaOrientation = actualRotationalVelocity*PERIOD;
+
+ x += cos(alpha+deltaOrientation)*deltaTranslation;
+ y += sin(alpha+deltaOrientation)*deltaTranslation;
+ float alpha = this->alpha+deltaOrientation;
+
+ while (alpha > PI) alpha -= 2.0f*PI;
+ while (alpha < -PI) alpha += 2.0f*PI;
+
+ this->alpha = alpha;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,79 @@
+/*
+ * Controller.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef CONTROLLER_H_
+#define CONTROLLER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+#include "EncoderCounter.h"
+#include "Motion.h"
+#include "LowpassFilter.h"
+
+/**
+ * This class implements the coordinate transformation, speed control and
+ * the position estimation of a mobile robot with differential drive.
+ */
+class Controller {
+
+ public:
+
+ Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight);
+ virtual ~Controller();
+ void setTranslationalVelocity(float velocity);
+ void setRotationalVelocity(float velocity);
+ float getActualTranslationalVelocity();
+ float getActualRotationalVelocity();
+ void setX(float x);
+ float getX();
+ void setY(float y);
+ float getY();
+ void setAlpha(float alpha);
+ float getAlpha();
+
+ private:
+
+ static const float PERIOD;
+ static const float PI;
+ static const float WHEEL_DISTANCE;
+ static const float WHEEL_RADIUS;
+ 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;
+ EncoderCounter& counterLeft;
+ EncoderCounter& counterRight;
+ Motion translationalMotion;
+ Motion rotationalMotion;
+ float translationalVelocity;
+ float rotationalVelocity;
+ float actualTranslationalVelocity;
+ float actualRotationalVelocity;
+ short previousValueCounterLeft;
+ short previousValueCounterRight;
+ LowpassFilter speedLeftFilter;
+ LowpassFilter speedRightFilter;
+ float desiredSpeedLeft;
+ float desiredSpeedRight;
+ float actualSpeedLeft;
+ float actualSpeedRight;
+ float x;
+ float y;
+ float alpha;
+ Ticker ticker;
+
+ void run();
+};
+
+#endif /* CONTROLLER_H_ */
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,170 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounter.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes 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.
+ */
+EncoderCounter::EncoderCounter(PinName a, PinName b) {
+
+ // check pins
+
+ if ((a == PA_0) && (b == PA_1)) {
+
+ // pinmap OK for TIM2 CH1 and CH2
+
+ TIM = TIM2;
+
+ // configure general purpose I/O registers
+
+ GPIOA->MODER &= ~GPIO_MODER_MODER0; // reset port A0
+ GPIOA->MODER |= GPIO_MODER_MODER0_1; // set alternate mode of port A0
+ GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR0; // reset pull-up/pull-down on port A0
+ GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1; // set input as pull-down
+ GPIOA->AFR[0] &= ~(0xF << 4*0); // reset alternate function of port A0
+ GPIOA->AFR[0] |= 1 << 4*0; // set alternate funtion 1 of port A0
+
+ GPIOA->MODER &= ~GPIO_MODER_MODER1; // reset port A1
+ GPIOA->MODER |= GPIO_MODER_MODER1_1; // set alternate mode of port A1
+ GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR1; // reset pull-up/pull-down on port A1
+ GPIOA->PUPDR |= GPIO_PUPDR_PUPDR1_1; // set input as pull-down
+ GPIOA->AFR[0] &= ~(0xF << 4*1); // reset alternate function of port A1
+ GPIOA->AFR[0] |= 1 << 4*1; // set alternate funtion 1 of port A1
+
+ // 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 == PA_6) && (b == PC_7)) {
+
+ // pinmap OK for TIM3 CH1 and CH2
+
+ TIM = TIM3;
+
+ // configure reset and clock control registers
+
+ RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // manually enable port C (port A enabled by mbed library)
+
+ // configure general purpose I/O registers
+
+ GPIOA->MODER &= ~GPIO_MODER_MODER6; // reset port A6
+ GPIOA->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port A6
+ GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port A6
+ GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down
+ GPIOA->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port A6
+ GPIOA->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port A6
+
+ 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 == PB_6) && (b == PB_7)) {
+
+ // pinmap OK for TIM4 CH1 and CH2
+
+ TIM = TIM4;
+
+ // 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
+
+ GPIOB->MODER &= ~GPIO_MODER_MODER6; // reset port B6
+ GPIOB->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port B6
+ GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port B6
+ GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down
+ GPIOB->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port B6
+ GPIOB->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port B6
+
+ GPIOB->MODER &= ~GPIO_MODER_MODER7; // reset port B7
+ GPIOB->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port B7
+ GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port B7
+ GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down
+ GPIOB->AFR[0] &= ~0xF0000000; // reset alternate function of port B7
+ GPIOB->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port B7
+
+ // 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");
+ }
+
+ // configure general purpose timer 3 or 4
+
+ 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
+}
+
+EncoderCounter::~EncoderCounter() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounter::reset() {
+
+ TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounter::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 EncoderCounter::read() {
+
+ return (short)(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounter::operator short() {
+
+ return read();
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,35 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_H_
+#define ENCODER_COUNTER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounter {
+
+ public:
+
+ EncoderCounter(PinName a, PinName b);
+ virtual ~EncoderCounter();
+ void reset();
+ void reset(short offset);
+ short read();
+ operator short();
+
+ private:
+
+ TIM_TypeDef* TIM;
+};
+
+#endif /* ENCODER_COUNTER_H_ */
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,218 @@
+/*
+ * IMU.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "IMU.h"
+
+using namespace std;
+
+const float IMU::PI = 3.14159265f; // the constant PI
+
+/**
+ * Creates an IMU object.
+ * @param spi a reference to an spi controller to use.
+ * @param csG the chip select output for the gyro sensor.
+ * @param csXM the chip select output for the accelerometer and the magnetometer.
+ */
+IMU::IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM) : spi(spi), csG(csG), csXM(csXM) {
+
+ // initialize SPI interface
+
+ spi.format(8, 3);
+ spi.frequency(1000000);
+
+ // reset chip select lines to logical high
+
+ csG = 1;
+ csXM = 1;
+
+ // initialize gyro
+
+ writeRegister(csG, CTRL_REG1_G, 0x0F); // enable gyro in all 3 axis
+
+ // initialize accelerometer
+
+ writeRegister(csXM, CTRL_REG0_XM, 0x00);
+ writeRegister(csXM, CTRL_REG1_XM, 0x5F);
+ writeRegister(csXM, CTRL_REG2_XM, 0x00);
+ writeRegister(csXM, CTRL_REG3_XM, 0x04);
+
+ // initialize magnetometer
+
+ writeRegister(csXM, CTRL_REG5_XM, 0x94);
+ writeRegister(csXM, CTRL_REG6_XM, 0x00);
+ writeRegister(csXM, CTRL_REG7_XM, 0x00);
+ writeRegister(csXM, CTRL_REG4_XM, 0x04);
+ writeRegister(csXM, INT_CTRL_REG_M, 0x09);
+}
+
+/**
+ * Deletes the IMU object.
+ */
+IMU::~IMU() {}
+
+/**
+ * This private method allows to write a register value.
+ * @param cs the chip select output to use, either csG or csXM.
+ * @param address the 7 bit address of the register.
+ * @param value the value to write into the register.
+ */
+void IMU::writeRegister(DigitalOut& cs, char address, char value) {
+
+ cs = 0;
+
+ spi.write(0x7F & address);
+ spi.write(value & 0xFF);
+
+ cs = 1;
+}
+
+/**
+ * This private method allows to read a register value.
+ * @param cs the chip select output to use, either csG or csXM.
+ * @param address the 7 bit address of the register.
+ * @return the value read from the register.
+ */
+char IMU::readRegister(DigitalOut& cs, char address) {
+
+ cs = 0;
+
+ spi.write(0x80 | address);
+ int value = spi.write(0xFF);
+
+ cs = 1;
+
+ return (char)(value & 0xFF);
+}
+
+/**
+ * Reads the gyroscope about the x-axis.
+ * @return the rotational speed about the x-axis given in [rad/s].
+ */
+float IMU::readGyroX() {
+
+ char low = readRegister(csG, OUT_X_L_G);
+ char high = readRegister(csG, OUT_X_H_G);
+
+ short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+
+ return (float)value/32768.0f*245.0f*PI/180.0f;
+}
+
+/**
+ * Reads the gyroscope about the y-axis.
+ * @return the rotational speed about the y-axis given in [rad/s].
+ */
+float IMU::readGyroY() {
+
+ char low = readRegister(csG, OUT_Y_L_G);
+ char high = readRegister(csG, OUT_Y_H_G);
+
+ short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+
+ return (float)value/32768.0f*245.0f*PI/180.0f;
+}
+
+/**
+ * Reads the gyroscope about the z-axis.
+ * @return the rotational speed about the z-axis given in [rad/s].
+ */
+float IMU::readGyroZ() {
+
+ char low = readRegister(csG, OUT_Z_L_G);
+ char high = readRegister(csG, OUT_Z_H_G);
+
+ short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+
+ return (float)value/32768.0f*245.0f*PI/180.0f;
+}
+
+/**
+ * Reads the acceleration in x-direction.
+ * @return the acceleration in x-direction, given in [m/s2].
+ */
+float IMU::readAccelerationX() {
+
+ char low = readRegister(csXM, OUT_X_L_A);
+ char high = readRegister(csXM, OUT_X_H_A);
+
+ short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+
+ return (float)value/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the acceleration in y-direction.
+ * @return the acceleration in y-direction, given in [m/s2].
+ */
+float IMU::readAccelerationY() {
+
+ char low = readRegister(csXM, OUT_Y_L_A);
+ char high = readRegister(csXM, OUT_Y_H_A);
+
+ short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+
+ return (float)value/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the acceleration in z-direction.
+ * @return the acceleration in z-direction, given in [m/s2].
+ */
+float IMU::readAccelerationZ() {
+
+ char low = readRegister(csXM, OUT_Z_L_A);
+ char high = readRegister(csXM, OUT_Z_H_A);
+
+ short value = (short)(((unsigned short)high << 8) | (unsigned short)low);
+
+ return (float)value/32768.0f*2.0f*9.81f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerX() {
+
+ // bitte implementieren!
+
+ return 0.0f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerY() {
+
+ // bitte implementieren!
+
+ return 0.0f;
+}
+
+/**
+ * Reads the magnetic field in x-direction.
+ * @return the magnetic field in x-direction, given in [Gauss].
+ */
+float IMU::readMagnetometerZ() {
+
+ // bitte implementieren!
+
+ return 0.0f;
+}
+
+/**
+ * Reads the compass heading about the z-axis.
+ * @return the compass heading in the range -PI to +PI, given in [rad].
+ */
+float IMU::readHeading() {
+
+ // bitte implementieren!
+
+ return 0.0f;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,74 @@
+/*
+ * IMU.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This is a device driver class for the ST LSM9DS0 inertial measurement unit.
+ */
+class IMU {
+
+ public:
+
+ IMU(SPI& spi, DigitalOut& csG, DigitalOut& csXM);
+ virtual ~IMU();
+ float readGyroX();
+ float readGyroY();
+ float readGyroZ();
+ float readAccelerationX();
+ float readAccelerationY();
+ float readAccelerationZ();
+ float readMagnetometerX();
+ float readMagnetometerY();
+ float readMagnetometerZ();
+ float readHeading();
+
+ private:
+
+ static const char WHO_AM_I_G = 0x0F;
+ static const char CTRL_REG1_G = 0x20;
+ static const char OUT_X_L_G = 0x28;
+ static const char OUT_X_H_G = 0x29;
+ static const char OUT_Y_L_G = 0x2A;
+ static const char OUT_Y_H_G = 0x2B;
+ static const char OUT_Z_L_G = 0x2C;
+ static const char OUT_Z_H_G = 0x2D;
+
+ static const char WHO_AM_I_XM = 0x0F;
+
+ static const char INT_CTRL_REG_M = 0x12;
+ static const char CTRL_REG0_XM = 0x1F;
+ static const char CTRL_REG1_XM = 0x20;
+ static const char CTRL_REG2_XM = 0x21;
+ static const char CTRL_REG3_XM = 0x22;
+ static const char CTRL_REG4_XM = 0x23;
+ static const char CTRL_REG5_XM = 0x24;
+ static const char CTRL_REG6_XM = 0x25;
+ static const char CTRL_REG7_XM = 0x26;
+
+ static const char OUT_X_L_A = 0x28;
+ static const char OUT_X_H_A = 0x29;
+ static const char OUT_Y_L_A = 0x2A;
+ static const char OUT_Y_H_A = 0x2B;
+ static const char OUT_Z_L_A = 0x2C;
+ static const char OUT_Z_H_A = 0x2D;
+
+ static const float PI;
+
+ SPI& spi;
+ DigitalOut& csG;
+ DigitalOut& csXM;
+
+ void writeRegister(DigitalOut& cs, char address, char value);
+ char readRegister(DigitalOut& cs, char address);
+};
+
+#endif /* IMU_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IRSensor.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,55 @@
+/*
+ * IRSensor.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "IRSensor.h"
+
+using namespace std;
+
+/**
+ * Creates an IRSensor object.
+ * @param distance an analog input object to read the voltage of the sensor.
+ * @param bit0 a digital output to set the first bit of the multiplexer.
+ * @param bit1 a digital output to set the second bit of the multiplexer.
+ * @param bit2 a digital output to set the third bit of the multiplexer.
+ * @param number the number of the sensor, either 0, 1, 2, 3, 4 or 5.
+ */
+IRSensor::IRSensor(AnalogIn& distance, DigitalOut& bit0, DigitalOut& bit1, DigitalOut& bit2, int number) : distance(distance), bit0(bit0), bit1(bit1), bit2(bit2) {
+
+ // set local references to objects
+
+ this->number = number;
+}
+
+/**
+ * Deletes the IRSensor object.
+ */
+IRSensor::~IRSensor() {}
+
+/**
+ * Gets the distance measured with the IR sensor in [m].
+ * @return the distance, given in [m].
+ */
+float IRSensor::read() {
+
+ bit0 = (number >> 0) & 1;
+ bit1 = (number >> 1) & 1;
+ bit2 = (number >> 2) & 1;
+
+ float d = -0.58f*sqrt(distance)+0.58f; // 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 Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,37 @@
+/*
+ * IRSensor.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef IR_SENSOR_H_
+#define IR_SENSOR_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This is a device driver class to read the distance measured with a Sharp IR sensor.
+ */
+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/LIDAR.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,207 @@
+/*
+ * LIDAR.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "LIDAR.h"
+
+using namespace std;
+
+/**
+ * Creates a LIDAR object.
+ * @param serial a reference to a serial interface to communicate with the laser scanner.
+ */
+LIDAR::LIDAR(RawSerial& serial) : serial(serial) {
+
+ // initialize serial interface
+
+ serial.baud(115200);
+ serial.format(8, SerialBase::None, 1);
+
+ // initialize local values
+
+ headerCounter = 0;
+ dataCounter = 0;
+
+ for (unsigned short i = 0; i < 360; i++) distances[i] = DEFAULT_DISTANCE;
+ distanceOfBeacon = 0;
+ angleOfBeacon = 0;
+
+ // start serial interrupt
+
+ serial.attach(callback(this, &LIDAR::receive), RawSerial::RxIrq);
+
+ // start the continuous operation of the LIDAR
+
+ serial.putc(START_FLAG);
+ serial.putc(SCAN);
+}
+
+/**
+ * Stops the lidar and deletes this object.
+ */
+LIDAR::~LIDAR() {
+
+ // stop the LIDAR
+
+ serial.putc(START_FLAG);
+ serial.putc(STOP);
+}
+
+/**
+ * Returns the distance measurement of the lidar at a given angle.
+ * @param angle the angle, given in [deg] in the range 0..359.
+ * @return the measured distance, given in [mm].
+ */
+short LIDAR::getDistance(short angle) {
+
+ while (angle < 0) angle += 360;
+ while (angle >= 360) angle -= 360;
+
+ return distances[angle];
+}
+
+/**
+ * Returns the distance to a detected beacon.
+ * @return the distance to the beacon, given in [mm], or zero, if no beacon was found.
+ */
+short LIDAR::getDistanceOfBeacon() {
+
+ return distanceOfBeacon;
+}
+
+/**
+ * Returns the angle of a detected beacon.
+ * @return the angle of the beacon, given in [deg] in the range 0..359.
+ */
+short LIDAR::getAngleOfBeacon() {
+
+ return angleOfBeacon;
+}
+
+/**
+ * This method implements an algorithm that looks for the position of a beacon.
+ * It should be called periodically by a low-priority background task.
+ */
+void LIDAR::lookForBeacon() {
+
+ distanceOfBeacon = 0;
+ angleOfBeacon = 0;
+ for (i=0;i<360;i++){
+ dist_copy[i] = distances[i]; //local copy for the search of lighthouses
+ }
+ n=0;
+ m=0;
+ changed=false;
+ for (i=0;i<10;i++){
+ dist_diff_start[i]=0; //
+ dist_diff_stop[i]=0;
+ }
+ for (i=0;i<360;i++){
+
+ if ((distances[i-1]-distances[i])>500 && distances[i]>500 && distances[i]<2000){
+ /**dist_diff_start[n]=i;
+ n++;
+
+ printf(" start: %d \r\n",i);*/
+
+ n=i;
+ changed=true;
+ }
+ if ((distances[i+1]-distances[i])>500&&distances[i]>500&&distances[i]<2000){
+ /**dist_diff_stop[m]=i;
+ m++;
+ printf(" stop: %d \r\n",i);*/
+ m=i;
+ changed=true;
+ }
+ if((m-n)<9&&(m-n)>2&& changed){
+ range=m-n;
+ max=0;
+ min=2000;
+ for(x=0;x<range;x++){
+ if(min> distances[n+x]){
+ min=distances[n+x];
+ }
+ else if (max< distances[n+x]){
+ max=distances[n+x];
+ }
+ }
+ if((max-min)<75&&(max-min)>0){
+ distanceOfBeacon=(max+min)/2;
+ angleOfBeacon=(n+m)/2;
+
+ // printf(" Fenster gefunden!! Start:%d Stop:%d max:%d min:%d \r \n",n,m,max,min);
+ changed=false;
+ }
+ }
+ }
+
+ /** for(i=0;i<10;i++){
+ printf(" %d: %d ", i,dist_diff_start[i]);
+ }
+ printf("\n\r") ;
+ for(i=0;i<10;i++){
+ printf(" %d: %d ", i,dist_diff_stop[i]);
+ }*/
+ printf("\n\r") ;
+ printf("\n\r") ;
+ printf("\n\r") ;
+
+
+
+
+
+
+ // bitte implementieren!
+}
+
+/**
+ * This method is called by the serial interrupt service routine.
+ * It handles the reception of measurements from the LIDAR.
+ */
+void LIDAR::receive() {
+
+ // read received characters while input buffer is full
+
+ if (serial.readable()) {
+
+ // read single character from serial interface
+
+ char c = serial.getc();
+
+ // add this character to the header or to the data buffer
+
+ if (headerCounter < HEADER_SIZE) {
+ headerCounter++;
+ } else {
+ if (dataCounter < DATA_SIZE) {
+ data[dataCounter] = c;
+ dataCounter++;
+ }
+ if (dataCounter >= DATA_SIZE) {
+
+ // data buffer is full, process measurement
+
+ char quality = data[0] >> 2;
+ short angle = 360-(((unsigned short)data[1] | ((unsigned short)data[2] << 8)) >> 1)/64;
+ int16_t distance = ((unsigned short)data[3] | ((unsigned short)data[4] << 8))/4;
+
+ if ((quality < QUALITY_THRESHOLD) || (distance < DISTANCE_THRESHOLD)) distance = DEFAULT_DISTANCE;
+
+ // store distance in [mm] into array of full scan
+
+ while (angle < 0) angle += 360;
+ while (angle >= 360) angle -= 360;
+ distances[angle] = distance;
+
+ // reset data counter
+
+ dataCounter = 0;
+ }
+ }
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDAR.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,71 @@
+/*
+ * LIDAR.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef LIDAR_H_
+#define LIDAR_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This is a device driver class for the Slamtec RP LIDAR A1.
+ */
+class LIDAR {
+
+ public:
+
+ LIDAR(RawSerial& serial);
+ virtual ~LIDAR();
+ short getDistance(short angle);
+ short getDistanceOfBeacon();
+ short getAngleOfBeacon();
+ void lookForBeacon();
+
+ private:
+
+ static const unsigned short HEADER_SIZE = 7;
+ static const unsigned short DATA_SIZE = 5;
+
+ static const char START_FLAG = 0xA5;
+ static const char SCAN = 0x20;
+ static const char STOP = 0x25;
+ static const char RESET = 0x40;
+
+ static const char QUALITY_THRESHOLD = 10;
+ static const short DISTANCE_THRESHOLD = 10;
+ static const short DEFAULT_DISTANCE = 10000;
+ static const short MIN_DISTANCE = 500;
+ static const short MAX_DISTANCE = 2000;
+ static const short THRESHOLD = 500;
+ static const short WINDOW = 75;
+ static const short MIN_SIZE = 2;
+ static const short MAX_SIZE = 9;
+
+ RawSerial& serial; // reference to serial interface for communication
+ char headerCounter;
+ char dataCounter;
+ char data[DATA_SIZE];
+ short distances[360]; // measured distance for every angle value, given in [mm]
+ short distanceOfBeacon; // distance of detected beacon, given in [mm]
+ short angleOfBeacon; // angle of detected beacon, given in [degrees]
+ short dist_copy[360];
+ int dist_diff_start[10];
+ int dist_diff_stop[10];
+ int i;
+ int n;
+ int m;
+ int x;
+ int range;
+ bool changed;
+ short min;
+ short max;
+
+
+ void receive();
+};
+
+#endif /* LIDAR_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,113 @@
+/*
+ * LowpassFilter.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "LowpassFilter.h"
+
+using namespace std;
+
+/**
+ * Creates a LowpassFilter object with a default cutoff frequency of 1000 [rad/s].
+ */
+LowpassFilter::LowpassFilter() {
+
+ period = 1.0f;
+ frequency = 1000.0f;
+
+ a11 = (1.0f+frequency*period)*exp(-frequency*period);
+ a12 = period*exp(-frequency*period);
+ a21 = -frequency*frequency*period*exp(-frequency*period);
+ a22 = (1.0f-frequency*period)*exp(-frequency*period);
+ b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+ b2 = period*exp(-frequency*period);
+
+ x1 = 0.0f;
+ x2 = 0.0f;
+}
+
+/**
+ * Deletes the LowpassFilter object.
+ */
+LowpassFilter::~LowpassFilter() {}
+
+/**
+ * Resets the filtered value to zero.
+ */
+void LowpassFilter::reset() {
+
+ x1 = 0.0f;
+ x2 = 0.0f;
+}
+
+/**
+ * Resets the filtered value to a given value.
+ * @param value the value to reset the filter to.
+ */
+void LowpassFilter::reset(float value) {
+
+ x1 = value/frequency/frequency;
+ x2 = (x1-a11*x1-b1*value)/a12;
+}
+
+/**
+ * Sets the sampling period of the filter.
+ * This is typically the sampling period of the periodic task of a controller that uses this filter.
+ * @param the sampling period, given in [s].
+ */
+void LowpassFilter::setPeriod(float period) {
+
+ this->period = period;
+
+ a11 = (1.0f+frequency*period)*exp(-frequency*period);
+ a12 = period*exp(-frequency*period);
+ a21 = -frequency*frequency*period*exp(-frequency*period);
+ a22 = (1.0f-frequency*period)*exp(-frequency*period);
+ b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+ b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Sets the cutoff frequency of this filter.
+ * @param frequency the cutoff frequency of the filter in [rad/s].
+ */
+void LowpassFilter::setFrequency(float frequency) {
+
+ this->frequency = frequency;
+
+ a11 = (1.0f+frequency*period)*exp(-frequency*period);
+ a12 = period*exp(-frequency*period);
+ a21 = -frequency*frequency*period*exp(-frequency*period);
+ a22 = (1.0f-frequency*period)*exp(-frequency*period);
+ b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
+ b2 = period*exp(-frequency*period);
+}
+
+/**
+ * Gets the current cutoff frequency of this filter.
+ * @return the current cutoff frequency in [rad/s].
+ */
+float LowpassFilter::getFrequency() {
+
+ return frequency;
+}
+
+/**
+ * Filters a value.
+ * @param value the original unfiltered value.
+ * @return the filtered value.
+ */
+float LowpassFilter::filter(float value) {
+
+ float x1old = x1;
+ float x2old = x2;
+
+ x1 = a11*x1old+a12*x2old+b1*value;
+ x2 = a21*x1old+a22*x2old+b2*value;
+
+ return frequency*frequency*x1;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LowpassFilter.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,40 @@
+/*
+ * LowpassFilter.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef LOWPASS_FILTER_H_
+#define LOWPASS_FILTER_H_
+
+#include <cstdlib>
+
+/**
+ * This class implements a time-discrete 2nd order lowpass filter for a series of data values.
+ * This filter can typically be used within a periodic task that takes measurements that need
+ * to be filtered, like speed or position values.
+ */
+class LowpassFilter {
+
+ public:
+
+ LowpassFilter();
+ virtual ~LowpassFilter();
+ void reset();
+ void reset(float value);
+ void setPeriod(float period);
+ void setFrequency(float frequency);
+ float getFrequency();
+ float filter(float value);
+
+ private:
+
+ float period;
+ float frequency;
+ float a11, a12, a21, a22, b1, b2;
+ float x1, x2;
+};
+
+#endif /* LOWPASS_FILTER_H_ */
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Main.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,114 @@
+/*
+ * Main.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cstdlib>
+#include <mbed.h>
+#include "EncoderCounter.h"
+#include "IRSensor.h"
+#include "IMU.h"
+#include "LIDAR.h"
+#include "Controller.h"
+#include "StateMachine.h"
+#include "SerialServer.h"
+
+int main() {
+
+
+
+ // create miscellaneous periphery objects
+
+ DigitalOut led(LED1);
+ DigitalIn button(USER_BUTTON);
+
+ DigitalOut led0(PC_8);
+ DigitalOut led1(PC_6);
+ DigitalOut led2(PB_12);
+ DigitalOut led3(PA_7);
+ DigitalOut led4(PC_0);
+ DigitalOut led5(PC_9);
+
+ // create motor control objects
+
+ DigitalOut enableMotorDriver(PB_2);
+ DigitalIn motorDriverFault(PB_14);
+ DigitalIn motorDriverWarning(PB_15);
+
+ PwmOut pwmLeft(PA_8);
+ PwmOut pwmRight(PA_9);
+
+ EncoderCounter counterLeft(PB_6, PB_7);
+ EncoderCounter counterRight(PA_6, PC_7);
+
+ // create distance sensor objects
+
+ DigitalOut enableIRSensors(PC_1);
+ enableIRSensors = 1;
+
+ AnalogIn distance(PB_1);
+ DigitalOut bit0(PH_1);
+ DigitalOut bit1(PC_2);
+ DigitalOut bit2(PC_3);
+
+ IRSensor irSensor0(distance, bit0, bit1, bit2, 0);
+ IRSensor irSensor1(distance, bit0, bit1, bit2, 1);
+ IRSensor irSensor2(distance, bit0, bit1, bit2, 2);
+ IRSensor irSensor3(distance, bit0, bit1, bit2, 3);
+ IRSensor irSensor4(distance, bit0, bit1, bit2, 4);
+ IRSensor irSensor5(distance, bit0, bit1, bit2, 5);
+
+ // create LIDAR object
+
+ PwmOut pwm(PA_10);
+ pwm.period(0.00005f);
+ pwm.write(0.5f); // 50% duty-cycle
+
+ RawSerial uart(PA_0, PA_1);
+
+ LIDAR lidar(uart);
+
+ // create robot controller objects
+
+ Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
+ StateMachine stateMachine(controller, enableMotorDriver, led0, led1, led2, led3, led4, led5, button, irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5);
+
+ // create serial server object
+
+ RawSerial serial(PB_10, PC_5);
+ serial.baud(9600);
+ serial.format(8, SerialBase::None, 1);
+
+ DigitalOut reset(PB_3);
+ DigitalOut modes1(PB_4);
+
+ modes1 = 0;
+
+ reset = 1; wait(0.1f);
+ reset = 0; wait(0.1f);
+ reset = 1; wait(0.1f);
+
+
+
+
+ SerialServer serialServer(serial, lidar, controller);
+
+
+ while (true) {
+
+ led = !led; // toggle led
+ led0 = !led0;
+ led1 = !led1;
+ led2 = !led2;
+ led3 = !led3;
+ led4 = !led4;
+ led5 = !led5;
+
+ //printf("hallo \r \n ");
+ wait(1.0f); // wait for 200 ms
+
+ lidar.lookForBeacon();
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motion.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,603 @@
+/*
+ * Motion.cpp
+ * Copyright (c) 2018, 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 Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,61 @@
+/*
+ * Motion.h
+ * Copyright (c) 2018, 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/SerialServer.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,141 @@
+/*
+ * SerialServer.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <vector>
+#include "SerialServer.h"
+
+using namespace std;
+
+inline string float2string(float f) {
+
+ char buffer[32];
+ sprintf(buffer, "%.3f", f);
+
+ return string(buffer);
+}
+
+inline string int2string(int i) {
+
+ char buffer[32];
+ sprintf(buffer, "%d", i);
+
+ return string(buffer);
+}
+
+const char SerialServer::INT_TO_CHAR[] = {0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46};
+
+/**
+ * Creates a serial server object.
+ */
+SerialServer::SerialServer(RawSerial& serial, LIDAR& lidar, Controller& controller) : serial(serial), lidar(lidar), controller(controller) {
+
+ input.clear();
+ output.clear();
+
+ serial.attach(callback(this, &SerialServer::receive), Serial::RxIrq);
+ //serial.attach(callback(this, &SerialServer::transmit), Serial::TxIrq);
+ ticker.attach(callback(this, &SerialServer::transmit), 0.001);
+}
+
+/**
+ * Deletes the serial server object.
+ */
+SerialServer::~SerialServer() {}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::receive() {
+
+ // read received characters while input buffer is full
+
+ while (serial.readable()) {
+ int c = serial.getc();
+ if (input.size() < BUFFER_SIZE) input += (char)c;
+ }
+
+ // check if input is complete (terminated with CR & LF)
+
+ if (input.find("\r\n") != string::npos) {
+
+ // parse request
+
+ string request = input.substr(0, input.find("\r\n"));
+ string name;
+ vector<string> values;
+
+ if (request.find(' ') != string::npos) {
+
+ name = request.substr(0, request.find(' '));
+ request = request.substr(request.find(' ')+1);
+
+ while (request.find(' ') != string::npos) {
+ values.push_back(request.substr(0, request.find(' ')));
+ request = request.substr(request.find(' ')+1);
+ }
+ values.push_back(request);
+
+ } else {
+
+ name = request;
+ }
+
+ input.clear();
+
+ // process request
+
+ if (name.compare("getDistance") == 0) {
+ short angle = atoi(values[0].c_str());
+ short distance = lidar.getDistance(angle);
+ output = "distance ";
+ for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
+ output += "\r\n";
+ } else if (name.compare("getBeacon") == 0) {
+ short angle = lidar.getAngleOfBeacon();
+ short distance = lidar.getDistanceOfBeacon();
+ output = "beacon ";
+ for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(angle >> (4*(3-i))) & 0x0F];
+ output += " ";
+ for (int i = 0; i < 4; i++) output += INT_TO_CHAR[(distance >> (4*(3-i))) & 0x0F];
+ output += "\r\n";
+ } else if (name.compare("getRobotPose") == 0) {
+ float x = controller.getX();
+ float y = controller.getY();
+ float alpha = controller.getAlpha();
+ output = "pose "+float2string(x)+" "+float2string(y)+" "+float2string(alpha)+"\r\n";
+ } else if (name.compare("getOrientation") == 0) {
+ float alpha = controller.getAlpha();
+ output = "orientation "+float2string(alpha)+"\r\n";
+ } else {
+ output = "request unknown\r\n";
+ }
+
+ // transmit first byte of output buffer
+
+ if (serial.writeable() && (output.size() > 0)) {
+ serial.putc(output[0]);
+ output.erase(0, 1);
+ }
+
+ } else if (input.size() >= BUFFER_SIZE) {
+
+ input.clear();
+ }
+}
+
+/**
+ * Callback method of serial interface.
+ */
+void SerialServer::transmit() {
+
+ // transmit output
+
+ while (serial.writeable() && (output.size() > 0)) {
+ serial.putc(output[0]);
+ output.erase(0, 1);
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SerialServer.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,45 @@
+/*
+ * SerialServer.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef SERIAL_SERVER_H_
+#define SERIAL_SERVER_H_
+
+#include <cstdlib>
+#include <string>
+#include <mbed.h>
+#include "LIDAR.h"
+#include "Controller.h"
+
+using namespace std;
+
+/**
+ * This class implements a communication server using a serial interface.
+ */
+class SerialServer {
+
+ public:
+
+ SerialServer(RawSerial& serial, LIDAR& lidar, Controller& controller);
+ virtual ~SerialServer();
+
+ private:
+
+ static const char INT_TO_CHAR[];
+ static const int BUFFER_SIZE = 64;
+
+ RawSerial& serial;
+ LIDAR& lidar;
+ Controller& controller;
+ string input;
+ string output;
+ Ticker ticker;
+
+ void receive();
+ void transmit();
+};
+
+#endif /* SERIAL_SERVER_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,192 @@
+/*
+ * StateMachine.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "StateMachine.h"
+
+using namespace std;
+
+const float StateMachine::PERIOD = 0.01f; // period of task in [s]
+const float StateMachine::DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
+const float StateMachine::TRANSLATIONAL_VELOCITY = 0.3f; // translational velocity in [m/s]
+const float StateMachine::ROTATIONAL_VELOCITY = 1.0f; // rotational velocity in [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) {
+
+ enableMotorDriver = 0;
+ state = ROBOT_OFF;
+ buttonNow = button;
+ buttonBefore = buttonNow;
+ taskList.clear();
+
+ ticker.attach(callback(this, &StateMachine::run), 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 periodically by the ticker object and implements the
+ * logic of the state machine.
+ */
+void StateMachine::run() {
+
+ // 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;
+ */
+ // implementation of the state machine
+
+ switch (state) {
+
+ case ROBOT_OFF:
+
+ buttonNow = button;
+
+ if (buttonNow && !buttonBefore) { // detect button rising edge
+
+ enableMotorDriver = 1;
+
+ taskList.push_back(new TaskWait(controller, 0.5f));
+ taskList.push_back(new TaskMove(controller, 0.1f, 0.0f));
+
+ state = PROCESSING_TASKS;
+ }
+
+ buttonBefore = buttonNow;
+
+ break;
+
+ case PROCESSING_TASKS:
+
+ buttonNow = button;
+
+ if (buttonNow && !buttonBefore) { // detect button rising edge
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(0.0f);
+
+ state = SLOWING_DOWN;
+
+ } else if ((irSensor0 < DISTANCE_THRESHOLD) || (irSensor1 < DISTANCE_THRESHOLD)) {
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
+
+ state = TURN_LEFT;
+
+ } else if (irSensor5 < DISTANCE_THRESHOLD) {
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
+
+ state = TURN_RIGHT;
+
+ } else if (taskList.size() > 0) {
+
+ Task* task = taskList.front();
+ int result = task->run(PERIOD);
+ if (result == Task::DONE) {
+ taskList.pop_front();
+ delete task;
+ }
+
+ } else {
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(0.0f);
+
+ state = SLOWING_DOWN;
+ }
+
+ buttonBefore = buttonNow;
+
+ break;
+
+ case TURN_LEFT:
+
+ buttonNow = button;
+
+ if (buttonNow && !buttonBefore) { // detect button rising edge
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(0.0f);
+
+ state = SLOWING_DOWN;
+
+ } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
+
+ state = PROCESSING_TASKS;
+ }
+
+ buttonBefore = buttonNow;
+
+ break;
+
+ case TURN_RIGHT:
+
+ buttonNow = button;
+
+ if (buttonNow && !buttonBefore) { // detect button rising edge
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(0.0f);
+
+ state = SLOWING_DOWN;
+
+ } else if ((irSensor0 > DISTANCE_THRESHOLD) && (irSensor1 > DISTANCE_THRESHOLD) && (irSensor5 > DISTANCE_THRESHOLD)) {
+
+ state = PROCESSING_TASKS;
+ }
+
+ buttonBefore = buttonNow;
+
+ break;
+
+ case SLOWING_DOWN:
+
+ if ((fabs(controller.getActualTranslationalVelocity()) < 0.01f) && (fabs(controller.getActualRotationalVelocity()) < 0.01f)) {
+
+ enableMotorDriver = 0;
+
+ while (taskList.size() > 0) {
+ delete taskList.front();
+ taskList.pop_front();
+ }
+
+ state = ROBOT_OFF;
+ }
+
+ break;
+
+ default:
+
+ state = ROBOT_OFF;
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/StateMachine.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,72 @@
+/*
+ * StateMachine.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef STATE_MACHINE_H_
+#define STATE_MACHINE_H_
+
+#include <cstdlib>
+#include <deque>
+#include <mbed.h>
+#include "Controller.h"
+#include "IRSensor.h"
+#include "Task.h"
+#include "TaskWait.h"
+#include "TaskMove.h"
+#include "TaskMoveTo.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 PROCESSING_TASKS = 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 float PERIOD; // period of task 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]
+
+ 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;
+ deque<Task*> taskList;
+ Ticker ticker;
+
+ void run();
+};
+
+#endif /* STATE_MACHINE_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Task.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,31 @@
+/*
+ * Task.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include "Task.h"
+
+using namespace std;
+
+/**
+ * Creates an abstract task object.
+ */
+Task::Task() {}
+
+/**
+ * Deletes the task object.
+ */
+Task::~Task() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * It contains the code this task has to work on.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int Task::run(float period) {
+
+ return DONE;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Task.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,30 @@
+/*
+ * Task.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_H_
+#define TASK_H_
+
+#include <cstdlib>
+
+/**
+ * This is an abstract task class with a method that
+ * is called periodically by a task sequencer.
+ */
+class Task {
+
+ public:
+
+ static const int FAULT = -1; /**< Task return value. */
+ static const int RUNNING = 0; /**< Task return value. */
+ static const int DONE = 1; /**< Task return value. */
+
+ Task();
+ virtual ~Task();
+ virtual int run(float period);
+};
+
+#endif /* TASK_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMove.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,74 @@
+/*
+ * TaskMove.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "TaskMove.h"
+
+using namespace std;
+
+const float TaskMove::DEFAULT_DURATION = 3600.0f;
+
+/**
+ * Creates a task object that moves the robot with given velocities.
+ * @param conroller a reference to the controller object of the robot.
+ * @param translationalVelocity the translational velocity, given in [m/s].
+ * @param rotationalVelocity the rotational velocity, given in [rad/s].
+ */
+TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity) : controller(controller) {
+
+ this->translationalVelocity = translationalVelocity;
+ this->rotationalVelocity = rotationalVelocity;
+ this->duration = DEFAULT_DURATION;
+
+ time = 0.0f;
+}
+
+/**
+ * Creates a task object that moves the robot with given velocities.
+ * @param conroller a reference to the controller object of the robot.
+ * @param translationalVelocity the translational velocity, given in [m/s].
+ * @param rotationalVelocity the rotational velocity, given in [rad/s].
+ * @param duration the duration to move the robot, given in [s].
+ */
+TaskMove::TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity, float duration) : controller(controller) {
+
+ this->translationalVelocity = translationalVelocity;
+ this->rotationalVelocity = rotationalVelocity;
+ this->duration = duration;
+
+ time = 0.0f;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskMove::~TaskMove() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int TaskMove::run(float period) {
+
+ time += period;
+
+ if (time < duration) {
+
+ controller.setTranslationalVelocity(translationalVelocity);
+ controller.setRotationalVelocity(rotationalVelocity);
+
+ return RUNNING;
+
+ } else {
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(0.0f);
+
+ return DONE;
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMove.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,38 @@
+/*
+ * TaskMove.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_MOVE_H_
+#define TASK_MOVE_H_
+
+#include <cstdlib>
+#include "Controller.h"
+#include "Task.h"
+
+/**
+ * This is a specific implementation of a task class that moves the robot with given velocities.
+ */
+class TaskMove : public Task {
+
+ public:
+
+ static const float DEFAULT_DURATION; /**< Default duration, given in [s]. */
+
+ TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity);
+ TaskMove(Controller& controller, float translationalVelocity, float rotationalVelocity, float duration);
+ virtual ~TaskMove();
+ virtual int run(float period);
+
+ private:
+
+ Controller& controller; // reference to controller object to use
+ float translationalVelocity; // translational velocity, given in [m/s]
+ float rotationalVelocity; // rotational velocity, given in [rad/s]
+ float duration; // duration to move the robot, given in [s]
+ float time; // current time, given in [s]
+};
+
+#endif /* TASK_MOVE_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMoveTo.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,113 @@
+/*
+ * TaskMoveTo.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include <cmath>
+#include "TaskMoveTo.h"
+
+using namespace std;
+
+const float TaskMoveTo::DEFAULT_VELOCITY = 0.2f; // default velocity value, given in [m/s]
+const float TaskMoveTo::DEFAULT_ZONE = 0.01f; // default zone value, given in [m]
+const float TaskMoveTo::PI = 3.14159265f; // the constant PI
+const float TaskMoveTo::K1 = 2.0f; // position controller gain parameter
+const float TaskMoveTo::K2 = 0.5f; // position controller gain parameter
+const float TaskMoveTo::K3 = 0.5f; // position controller gain parameter
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the target position, given in [m].
+ * @param y the y coordinate of the target position, given in [m].
+ * @param alpha the target orientation, given in [rad].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha) : controller(controller) {
+
+ this->x = x;
+ this->y = y;
+ this->alpha = alpha;
+ this->velocity = DEFAULT_VELOCITY;
+ this->zone = DEFAULT_ZONE;
+}
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the target position, given in [m].
+ * @param y the y coordinate of the target position, given in [m].
+ * @param alpha the target orientation, given in [rad].
+ * @param velocity the maximum translational velocity, given in [m/s].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity) : controller(controller) {
+
+ this->x = x;
+ this->y = y;
+ this->alpha = alpha;
+ this->velocity = velocity;
+ this->zone = DEFAULT_ZONE;
+}
+
+/**
+ * Creates a task object that moves the robot to a given pose.
+ * @param conroller a reference to the controller object of the robot.
+ * @param x the x coordinate of the target position, given in [m].
+ * @param y the y coordinate of the target position, given in [m].
+ * @param alpha the target orientation, given in [rad].
+ * @param velocity the maximum translational velocity, given in [m/s].
+ * @param zone the zone threshold around the target position, given in [m].
+ */
+TaskMoveTo::TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone) : controller(controller) {
+
+ this->x = x;
+ this->y = y;
+ this->alpha = alpha;
+ this->velocity = velocity;
+ this->zone = zone;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskMoveTo::~TaskMoveTo() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int TaskMoveTo::run(float period) {
+
+ float translationalVelocity = 0.0f;
+ float rotationalVelocity = 0.0f;
+
+ float x = controller.getX();
+ float y = controller.getY();
+ float alpha = controller.getAlpha();
+
+ float rho = sqrt((this->x-x)*(this->x-x)+(this->y-y)*(this->y-y));
+
+ if (rho > 0.001f) {
+
+ float gamma = atan2(this->y-y, this->x-x)-alpha;
+
+ while (gamma < -PI) gamma += 2.0f*PI;
+ while (gamma > PI) gamma -= 2.0f*PI;
+
+ float delta = gamma+alpha-this->alpha;
+
+ while (delta < -PI) delta += 2.0f*PI;
+ while (delta > PI) delta -= 2.0f*PI;
+
+ translationalVelocity = K1*rho*cos(gamma);
+ translationalVelocity = (translationalVelocity > velocity) ? velocity : (translationalVelocity < -velocity) ? -velocity : translationalVelocity;
+ rotationalVelocity = (fabs(gamma) > 0.001f) ? K2*gamma+K1*sin(gamma)*cos(gamma)*(gamma+K3*delta)/gamma : 0.0f;
+ }
+
+ controller.setTranslationalVelocity(translationalVelocity);
+ controller.setRotationalVelocity(rotationalVelocity);
+
+ return (rho < zone) ? DONE : RUNNING;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskMoveTo.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,46 @@
+/*
+ * TaskMoveTo.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_MOVE_TO_H_
+#define TASK_MOVE_TO_H_
+
+#include <cstdlib>
+#include "Controller.h"
+#include "Task.h"
+
+/**
+ * This is a specific implementation of a task class that moves the robot to a given pose.
+ */
+class TaskMoveTo : public Task {
+
+ public:
+
+ static const float DEFAULT_VELOCITY; /**< Default velocity value, given in [m/s]. */
+ static const float DEFAULT_ZONE; /**< Default zone value, given in [m]. */
+
+ TaskMoveTo(Controller& controller, float x, float y, float alpha);
+ TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity);
+ TaskMoveTo(Controller& controller, float x, float y, float alpha, float velocity, float zone);
+ virtual ~TaskMoveTo();
+ virtual int run(float period);
+
+ private:
+
+ static const float PI;
+ static const float K1;
+ static const float K2;
+ static const float K3;
+
+ Controller& controller; // reference to the controller object to use
+ float x; // x coordinate of target position, given in [m]
+ float y; // y coordinate of target position, given in [m]
+ float alpha; // target orientation, given in [rad]
+ float velocity; // maximum translational velocity, given in [m/s]
+ float zone; // zone threshold around target position, given in [m]
+};
+
+#endif /* TASK_MOVE_TO_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskWait.cpp Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,47 @@
+/*
+ * TaskWait.cpp
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#include "TaskWait.h"
+
+using namespace std;
+
+/**
+ * Creates a task object that waits for a given duration.
+ */
+TaskWait::TaskWait(Controller& controller, float duration) : controller(controller) {
+
+ this->duration = duration;
+
+ time = 0.0f;
+}
+
+/**
+ * Deletes the task object.
+ */
+TaskWait::~TaskWait() {}
+
+/**
+ * This method is called periodically by a task sequencer.
+ * @param period the period of the task sequencer, given in [s].
+ * @return the status of this task, i.e. RUNNING or DONE.
+ */
+int TaskWait::run(float period) {
+
+ controller.setTranslationalVelocity(0.0f);
+ controller.setRotationalVelocity(0.0f);
+
+ time += period;
+
+ if (time < duration) {
+
+ return RUNNING;
+
+ } else {
+
+ return DONE;
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TaskWait.h Fri Apr 27 08:47:34 2018 +0000
@@ -0,0 +1,33 @@
+/*
+ * TaskWait.h
+ * Copyright (c) 2018, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef TASK_WAIT_H_
+#define TASK_WAIT_H_
+
+#include <cstdlib>
+#include "Controller.h"
+#include "Task.h"
+
+/**
+ * This is a specific implementation of a task class that waits for a given duration.
+ */
+class TaskWait : public Task {
+
+ public:
+
+ TaskWait(Controller& controller, float duration);
+ virtual ~TaskWait();
+ virtual int run(float period);
+
+ private:
+
+ Controller& controller;
+ float duration;
+ float time;
+};
+
+#endif /* TASK_WAIT_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Apr 27 08:47:34 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee \ No newline at end of file