Michael Ernst Peter
Example project
Dependencies: PM2_Libary Eigen
Revision 42:d2d2db5974c9, committed 2022-05-14
- Comitter:
- pmic
- Date:
- Sat May 14 15:27:12 2022 +0200
- Parent:
- 41:7484471403aa
- Child:
- 43:0a124a21e227
- Commit message:
- Working solution, only mbed update left
Changed in this revision
--- a/Controller.cpp Wed May 11 09:44:25 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,192 +0,0 @@
-/*
- * 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), std::chrono::microseconds{static_cast<long int>(1.0e6f * 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;
- }
-}
--- a/Controller.h Wed May 11 09:44:25 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,76 +0,0 @@
-/*
- * 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_ */
--- a/EncoderCounterROME2.cpp Wed May 11 09:44:25 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,185 +0,0 @@
-/*
- * 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();
-}
--- a/EncoderCounterROME2.h Wed May 11 09:44:25 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,33 +0,0 @@
-/*
- * 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_ */
--- a/IRSensor.cpp Wed May 11 09:44:25 2022 +0200
+++ b/IRSensor.cpp Sat May 14 15:27:12 2022 +0200
@@ -45,9 +45,9 @@
/**
* The empty operator is a shorthand notation of the <code>read()</code> method.
*/
-/*
+ /*
IRSensor::operator float() {
return read();
}
-*/
\ No newline at end of file
+*/
--- a/IRSensor.h Wed May 11 09:44:25 2022 +0200
+++ b/IRSensor.h Sat May 14 15:27:12 2022 +0200
@@ -21,7 +21,7 @@
IRSensor(AnalogIn& distance, DigitalOut& bit0, DigitalOut& bit1, DigitalOut& bit2, int number);
virtual ~IRSensor();
float read();
- // operator float();
+ //operator float();
private:
--- a/Motion.cpp Wed May 11 09:44:25 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,601 +0,0 @@
-/*
- * 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;
- }
- }
- }
-}
--- a/Motion.h Wed May 11 09:44:25 2022 +0200
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,59 +0,0 @@
-/*
- * 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_ */
--- a/PM2_Libary.lib Wed May 11 09:44:25 2022 +0200 +++ b/PM2_Libary.lib Sat May 14 15:27:12 2022 +0200 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/pmic/code/PM2_Libary/#6b30640c9e2e +https://os.mbed.com/users/pmic/code/PM2_Libary/#05abc1d2a2b90c1153c16504c84bdc9d3cd7d809 \ No newline at end of file
--- a/main.cpp Wed May 11 09:44:25 2022 +0200
+++ b/main.cpp Sat May 14 15:27:12 2022 +0200
@@ -1,79 +1,113 @@
-#include <stdio.h>
#include <mbed.h>
+#include <math.h>
#include <vector>
+#include "PM2_Libary.h"
+#include "Eigen/Dense.h"
+
#include "IRSensor.h"
-#include "EncoderCounterROME2.h"
-#include "Controller.h"
-
-#include "Eigen/Dense.h"
# define M_PI 3.14159265358979323846 // number pi
+/**
+ * notes:
+ * - IRSensor class is not available in PM2_Libary
+ * - ROME2 Robot uses different PINS than PES board
+ */
+
// logical variable main task
bool do_execute_main_task = false; // this variable will be toggled via the user button (blue button) to or not to execute the main task
// user button on nucleo board
Timer user_button_timer; // create Timer object which we use to check if user button was pressed for a certain time (robust against signal bouncing)
-InterruptIn user_button(BUTTON1); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
+InterruptIn user_button(PC_13); // create InterruptIn interface object to evaluate user button falling and rising edge (no blocking code in ISR)
void user_button_pressed_fcn(); // custom functions which gets executed when user button gets pressed and released, definition below
void user_button_released_fcn();
-// while loop gets executed every main_task_period_ms milliseconds
-int main_task_period_ms = 50; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
-Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms
+int main() {
+
+ // while loop gets executed every main_task_period_ms milliseconds
+ const int main_task_period_ms = 10; // define main task period time in ms e.g. 50 ms -> main task runns 20 times per second
+ Timer main_task_timer; // create Timer object which we use to run the main task every main task period time in ms
-// led on nucleo board
-DigitalOut user_led(LED1); // create DigitalOut object to command user led
-
-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;
+ // led on nucleo board
+ DigitalOut user_led(LED1); // create DigitalOut object to command user led
-const float DISTANCE_THRESHOLD = 0.4f; // minimum allowed distance to obstacle in [m]
-const float TRANSLATIONAL_VELOCITY = 1.0f; // translational velocity in [m/s]
-const float ROTATIONAL_VELOCITY = 1.0f; // rotational velocity in [rad/s]
-const float VELOCITY_THRESHOLD = 0.01; // velocity threshold before switching off, in [m/s] and [rad/s]
+ // discrete states of this state machine
+ const int ROBOT_OFF = 0;
+ const int MOVE_FORWARD = 1;
+ const int TURN_LEFT = 2;
+ const int TURN_RIGHT = 3;
+ const int SLOWING_DOWN = 4;
-DigitalOut led0(PD_4);
-DigitalOut led1(PD_3);
-DigitalOut led2(PD_6);
-DigitalOut led3(PD_2);
-DigitalOut led4(PD_7);
-DigitalOut led5(PD_5);
-std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5};
+ // default parameters for robots movement
+ const float DISTANCE_THRESHOLD = 0.2f; // minimum allowed distance to obstacle in [m]
+ const float TRANSLATIONAL_VELOCITY = 0.4f; // translational velocity in [m/s]
+ const float ROTATIONAL_VELOCITY = 1.6f; // rotational velocity in [rad/s]
+ const float VELOCITY_THRESHOLD = 0.05; // velocity threshold before switching off, in [m/s] and [rad/s]
+
+ // create DigitalOut objects for leds
+ DigitalOut led0(PC_8);
+ DigitalOut led1(PC_6);
+ DigitalOut led2(PB_12);
+ DigitalOut led3(PA_7);
+ DigitalOut led4(PC_0);
+ DigitalOut led5(PC_9);
+ std::vector<DigitalOut> leds = {led0, led1, led2, led3, led4, led5};
-// create IR sensor objects
-AnalogIn dist(PA_0);
-DigitalOut enable(PG_1);
-DigitalOut bit0(PF_0);
-DigitalOut bit1(PF_1);
-DigitalOut bit2(PF_2);
+ // create IR sensor objects
+ AnalogIn dist(PB_1);
+ DigitalOut enable_leds(PC_1);
+ DigitalOut bit0(PH_1);
+ DigitalOut bit1(PC_2);
+ DigitalOut bit2(PC_3);
+ IRSensor irSensor0(dist, bit0, bit1, bit2, 0);
+ IRSensor irSensor1(dist, bit0, bit1, bit2, 1);
+ IRSensor irSensor2(dist, bit0, bit1, bit2, 2);
+ IRSensor irSensor3(dist, bit0, bit1, bit2, 3);
+ IRSensor irSensor4(dist, bit0, bit1, bit2, 4);
+ IRSensor irSensor5(dist, bit0, bit1, bit2, 5);
+ std::vector<IRSensor> irSensors = {irSensor0, irSensor1, irSensor2, irSensor3, irSensor4, irSensor5};
-IRSensor irSensor0(dist, bit0, bit1, bit2, 0);
-IRSensor irSensor1(dist, bit0, bit1, bit2, 1);
-IRSensor irSensor2(dist, bit0, bit1, bit2, 2);
-IRSensor irSensor3(dist, bit0, bit1, bit2, 3);
-IRSensor irSensor4(dist, bit0, bit1, bit2, 4);
-IRSensor irSensor5(dist, bit0, bit1, bit2, 5);
+ // 19:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion)
+ DigitalOut enable_motors(PB_2);
+ DigitalIn motorDriverFault(PB_14);
+ DigitalIn motorDriverWarning(PB_15);
-// create motor control objects
-DigitalOut enableMotorDriver(PG_0);
-DigitalIn motorDriverFault(PD_1);
-DigitalIn motorDriverWarning(PD_0);
-PwmOut pwmLeft(PF_9);
-PwmOut pwmRight(PF_8);
-EncoderCounterROME2 counterLeft(PD_12, PD_13);
-EncoderCounterROME2 counterRight(PB_4, PC_7);
+ // create SpeedController objects
+ FastPWM pwm_M1(PA_9); // motor M1 is closed-loop speed controlled (angle velocity)
+ FastPWM pwm_M2(PA_8); // motor M2 is closed-loop speed controlled (angle velocity)
+ EncoderCounter encoder_M1(PA_6, PC_7); // create encoder objects to read in the encoder counter values
+ EncoderCounter encoder_M2(PB_6, PB_7);
+ const float max_voltage = 12.0f; // define maximum voltage of battery packs, adjust this to 6.0f V if you only use one batterypack
+ const float counts_per_turn = 64.0f * 19.0f; // define counts per turn at gearbox end: counts/turn * gearratio
+ const float kn = 530.0f / 12.0f; // define motor constant in rpm per V
+
+ SpeedController* speedControllers[2];
+ speedControllers[0] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
+ speedControllers[1] = new SpeedController(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
+ speedControllers[0]->setSpeedCntrlGain(0.04f); // adjust speedcontroller gains
+ speedControllers[1]->setSpeedCntrlGain(0.04f);
+ speedControllers[0]->setMaxAccelerationRPS(10.0f); // adjust max. acceleration for smooth movement
+ speedControllers[1]->setMaxAccelerationRPS(10.0f);
-int state;
-// 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);
-
-int main() {
+ // robot kinematics
+ const float r_wheel = 0.0766f / 2.0f; // wheel radius
+ const float L_wheel = 0.176f; // distance from wheel to wheel
+ Eigen::Matrix2f Cwheel2robot; // transform wheel to robot
+ //Eigen::Matrix2f Crobot2wheel; // transform robot to wheel
+ Cwheel2robot << -r_wheel / 2.0f , r_wheel / 2.0f ,
+ -r_wheel / L_wheel, -r_wheel / L_wheel;
+ //Crobot2wheel << -1.0f / r_wheel, -L_wheel / (2.0f * r_wheel),
+ // 1.0f / r_wheel, -L_wheel / (2.0f * r_wheel);
+ Eigen::Vector2f robot_coord; // contains v and w (robot translational and rotational velocities)
+ Eigen::Vector2f wheel_speed; // w1 w2 (wheel speed)
+ Eigen::Vector2f wheel_speed_actual;
+ Eigen::Vector2f robot_coord_actual;
+ robot_coord.setZero();
+ wheel_speed.setZero();
+ wheel_speed_actual.setZero();
+ robot_coord_actual.setZero();
// attach button fall and rise functions to user button object
user_button.fall(&user_button_pressed_fcn);
@@ -81,159 +115,110 @@
// start timer
main_task_timer.start();
-
- enable = 1;
-
- enableMotorDriver = 0;
- state = ROBOT_OFF;
- Eigen::Matrix<float, 6, 1> irSensor_distance; // transform wheel to robot
+ // set initial state machine state, enalbe leds, disable motors
+ int state = ROBOT_OFF;
+ enable_leds = 1;
+ enable_motors = 0;
while (true) { // this loop will run forever
main_task_timer.reset();
- // read the distance sensors
- irSensor_distance << irSensor0.read(),
- irSensor1.read(),
- irSensor2.read(),
- irSensor3.read(),
- irSensor4.read(),
- irSensor5.read();
-
- // set the leds based on distance measurements
- ///*
- // iterator based foor loop
- int cnt = 0;
- for(auto it = leds.begin(); it != leds.end(); it++){
- *it = irSensor_distance(cnt) < DISTANCE_THRESHOLD;
- cnt++;
+ // set leds according to DISTANCE_THRESHOLD
+ for (uint8_t i = 0; i < leds.size(); i++) {
+ if (irSensors[i].read() > DISTANCE_THRESHOLD)
+ leds[i] = 0;
+ else
+ leds[i] = 1;
}
- //*/
- /*
- // index based for loop
- for (int i = 0; i < leds.size(); i++) {
- leds[i] = irSensor_distance(i) < DISTANCE_THRESHOLD;
- }
- */
- /*
- // range based for loop
- int cnt = 0;
- for(DigitalOut led : leds){
- led = irSensor_distance(cnt) < DISTANCE_THRESHOLD;
- cnt++;
- }
- /*
- led0 = irSensor_distance(0) < DISTANCE_THRESHOLD;
- led1 = irSensor_distance(1) < DISTANCE_THRESHOLD;
- led2 = irSensor_distance(2) < DISTANCE_THRESHOLD;
- led3 = irSensor_distance(3) < DISTANCE_THRESHOLD;
- led4 = irSensor_distance(4) < DISTANCE_THRESHOLD;
- led5 = irSensor_distance(5) < DISTANCE_THRESHOLD;
- */
+ // read actual wheel speed and transform it to robot coordinates
+ wheel_speed_actual << speedControllers[0]->getSpeedRPS(), speedControllers[1]->getSpeedRPS();
+ robot_coord_actual = Cwheel2robot * wheel_speed_actual;
+
+ // state machine
switch (state) {
- // enables Motors, sets translational speed and switches to MOVE_FORWARD
- case ROBOT_OFF:
+ case ROBOT_OFF:
+
if (do_execute_main_task) {
- enableMotorDriver = 1;
- controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
- controller.setRotationalVelocity(0.0f);
+ enable_motors = 1;
+ robot_coord(0) = TRANSLATIONAL_VELOCITY;
+ robot_coord(1) = 0.0f;
state = MOVE_FORWARD;
}
break;
-
- // continue here
+
case MOVE_FORWARD:
+
if (!do_execute_main_task) {
- controller.setTranslationalVelocity(0);
- controller.setRotationalVelocity(0);
+ robot_coord(0) = 0.0f;
+ robot_coord(1) = 0.0f;
state = SLOWING_DOWN;
- break;
- }
-
- // ???
- if (irSensor_distance(2) < DISTANCE_THRESHOLD && irSensor_distance(2) < DISTANCE_THRESHOLD) {
- controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2);
- controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
+ } else if ((irSensors[0].read() < DISTANCE_THRESHOLD) || (irSensors[1].read() < DISTANCE_THRESHOLD)) {
+ robot_coord(0) = 0.0f;
+ robot_coord(1) = ROTATIONAL_VELOCITY;
+ state = TURN_LEFT;
+ } else if (irSensors[5].read() < DISTANCE_THRESHOLD) {
+ robot_coord(0) = 0.0f;
+ robot_coord(1) = -ROTATIONAL_VELOCITY;
state = TURN_RIGHT;
- break;
- // ???
- } else if (irSensor_distance(4) < DISTANCE_THRESHOLD && irSensor_distance(4) < DISTANCE_THRESHOLD) {
- controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY/2);
- controller.setRotationalVelocity(ROTATIONAL_VELOCITY);
- state = TURN_LEFT;
- break;
- } else if (irSensor_distance(3) < DISTANCE_THRESHOLD/2) {
- controller.setTranslationalVelocity(-TRANSLATIONAL_VELOCITY);
- controller.setRotationalVelocity(-ROTATIONAL_VELOCITY);
- state = TURN_RIGHT;
- break;
+ } else {
+ // leave it driving
}
-
break;
case TURN_LEFT:
+
if (!do_execute_main_task) {
- controller.setTranslationalVelocity(0);
- controller.setRotationalVelocity(0);
+ robot_coord(1) = 0.0f;
state = SLOWING_DOWN;
- break;
- }
- if ( (irSensor_distance(2) > DISTANCE_THRESHOLD) && (irSensor_distance(3) > DISTANCE_THRESHOLD) && (irSensor_distance(4) > DISTANCE_THRESHOLD) ) {
- controller.setRotationalVelocity(0);
- controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+ } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
+ robot_coord(0) = TRANSLATIONAL_VELOCITY;
+ robot_coord(1) = 0.0f;
state = MOVE_FORWARD;
- break;
}
break;
- case TURN_RIGHT:
+ case TURN_RIGHT:
+
if (!do_execute_main_task) {
- controller.setTranslationalVelocity(0);
- controller.setRotationalVelocity(0);
+ robot_coord(1) = 0.0f;
state = SLOWING_DOWN;
- break;
- }
-
- if ( (irSensor_distance(2) > DISTANCE_THRESHOLD) && (irSensor_distance(3) > DISTANCE_THRESHOLD) && (irSensor_distance(4) > DISTANCE_THRESHOLD) ) {
- controller.setRotationalVelocity(0);
- controller.setTranslationalVelocity(TRANSLATIONAL_VELOCITY);
+ } else if ((irSensors[0].read() > DISTANCE_THRESHOLD) && (irSensors[1].read() > DISTANCE_THRESHOLD) && (irSensors[5].read() > DISTANCE_THRESHOLD)) {
+ robot_coord(0) = TRANSLATIONAL_VELOCITY;
+ robot_coord(1) = 0.0f;
state = MOVE_FORWARD;
- break;
}
break;
case SLOWING_DOWN:
- if (abs(controller.getActualTranslationalVelocity()) < VELOCITY_THRESHOLD
- && abs(controller.getActualRotationalVelocity()) > VELOCITY_THRESHOLD) {
- state = ROBOT_OFF;
- enableMotorDriver = 0;
+
+ if ((fabs(robot_coord_actual(0)) < VELOCITY_THRESHOLD) && (fabs(robot_coord_actual(1)) < VELOCITY_THRESHOLD)) {
+ enable_motors = 0;
state = ROBOT_OFF;
}
break;
-
- default:
-
- state = ROBOT_OFF;
+
}
+ // transform robot coordinates to wheel speed
+ wheel_speed = Cwheel2robot.inverse() * robot_coord;
+
+ // command speedController objects
+ speedControllers[0]->setDesiredSpeedRPS(wheel_speed(0) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M1
+ speedControllers[1]->setDesiredSpeedRPS(wheel_speed(1) / (2.0f * M_PI)); // set a desired speed for speed controlled dc motors M2
+
user_led = !user_led;
- /*
- if (do_execute_main_task)
- printf("button pressed\r\n");
- else
- printf("button NOT pressed\r\n");
- */
- // printf("actual velocity: %.3f [m/s] / %.3f [rad/s]\r\n", controller.getActualTranslationalVelocity(), controller.getActualRotationalVelocity());
- printf("%f, %f, %f, %f, %f, %f\r\n", irSensor_distance(0), irSensor_distance(1), irSensor_distance(2), irSensor_distance(3), irSensor_distance(4), irSensor_distance(5));
+ // do only output via serial what's really necessary (this makes your code slow)
+ printf("%f, %f\r\n", wheel_speed_actual(0), wheel_speed_actual(1));
// read timer and make the main thread sleep for the remaining time span (non blocking)
- auto main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
+ int main_task_elapsed_time_ms = std::chrono::duration_cast<std::chrono::milliseconds>(main_task_timer.elapsed_time()).count();
thread_sleep_for(main_task_period_ms - main_task_elapsed_time_ms);
}
}
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 |
