NagaokaRoboticsClub_mbedTeam / S-ShapeModelFloat

S-curve acceleration / deceleration model generator (using FPU)

Dependents:   2021NHK_B_main

position_controller.cpp@1:aed2cca33061, 2018-08-03 (annotated)

Committer:
UCHITAKE
Date:
Fri Aug 03 03:57:44 2018 +0000
Revision:
1:aed2cca33061
Child:
2:2c5e4f521390
??????xy(???)???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 1:aed2cca33061 1 #include "position_controller.h"
UCHITAKE 1:aed2cca33061 2
UCHITAKE 1:aed2cca33061 3 PositionController::PositionController(int accDist, float maxV, int targetX, int targetY) :
UCHITAKE 1:aed2cca33061 4 accelerationDistance(accDist),
UCHITAKE 1:aed2cca33061 5 maxVelocity(maxV),
UCHITAKE 1:aed2cca33061 6 targetDistance(hypot(targetX, targetY)),
UCHITAKE 1:aed2cca33061 7 targetDistanceX(targetX),
UCHITAKE 1:aed2cca33061 8 targetDistanceY(targetY),
UCHITAKE 1:aed2cca33061 9 targetRadians(atan2(targetY, targetX))
UCHITAKE 1:aed2cca33061 10 {
UCHITAKE 1:aed2cca33061 11 if(targetDistance > accelerationDistance * 2.0) {
UCHITAKE 1:aed2cca33061 12 maxJerk = (maxVelocity / 2.0) / (pow(accelerationDistance / 2.0, 2));
UCHITAKE 1:aed2cca33061 13 } else {
UCHITAKE 1:aed2cca33061 14 maxJerk = (maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0))/2.0) / (pow(targetDistance / 4.0, 2));
UCHITAKE 1:aed2cca33061 15 }
UCHITAKE 1:aed2cca33061 16 }
UCHITAKE 1:aed2cca33061 17
UCHITAKE 1:aed2cca33061 18 float PositionController::generateVelocity(int position)
UCHITAKE 1:aed2cca33061 19 {
UCHITAKE 1:aed2cca33061 20 float vel;
UCHITAKE 1:aed2cca33061 21 if(targetDistance > accelerationDistance * 2.0) {
UCHITAKE 1:aed2cca33061 22 if(position >= 0 && position < accelerationDistance / 2.0) {
UCHITAKE 1:aed2cca33061 23 //acc acc
UCHITAKE 1:aed2cca33061 24 vel = pow(position, 2) * maxJerk;
UCHITAKE 1:aed2cca33061 25 } else if(position >= accelerationDistance / 2.0 && position < accelerationDistance) {
UCHITAKE 1:aed2cca33061 26 //acc dec
UCHITAKE 1:aed2cca33061 27 vel = pow(position - accelerationDistance, 2) * (-maxJerk) + maxVelocity;
UCHITAKE 1:aed2cca33061 28 } else if(position >= accelerationDistance && position < targetDistance - accelerationDistance) {
UCHITAKE 1:aed2cca33061 29 //max velocity
UCHITAKE 1:aed2cca33061 30 vel = maxVelocity;
UCHITAKE 1:aed2cca33061 31 } else if(position >= targetDistance - accelerationDistance && position < targetDistance - accelerationDistance / 2.0) {
UCHITAKE 1:aed2cca33061 32 //dec acc
UCHITAKE 1:aed2cca33061 33 vel = pow(position - (targetDistance - accelerationDistance), 2) * (-maxJerk) + maxVelocity;
UCHITAKE 1:aed2cca33061 34 } else if(position >= targetDistance - accelerationDistance / 2.0){
UCHITAKE 1:aed2cca33061 35 //dec dec
UCHITAKE 1:aed2cca33061 36 vel = pow(position - targetDistance, 2) * maxJerk;
UCHITAKE 1:aed2cca33061 37 }
UCHITAKE 1:aed2cca33061 38 } else {
UCHITAKE 1:aed2cca33061 39 if(position >= 0 && position < targetDistance / 4.0) {
UCHITAKE 1:aed2cca33061 40 vel = pow(position, 2) * maxJerk;
UCHITAKE 1:aed2cca33061 41 } else if(position >= targetDistance / 4.0 && position < targetDistance / 2.0) {
UCHITAKE 1:aed2cca33061 42 //acc dec
UCHITAKE 1:aed2cca33061 43 vel = pow(position - targetDistance / 2.0, 2) * (-maxJerk) + maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0));
UCHITAKE 1:aed2cca33061 44 } else if(position >= targetDistance / 2.0 && position < targetDistance * (3.0/4.0)) {
UCHITAKE 1:aed2cca33061 45 //dec acc
UCHITAKE 1:aed2cca33061 46 vel = pow(position - targetDistance / 2.0, 2) * (-maxJerk) + maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0));
UCHITAKE 1:aed2cca33061 47 } else if(position >= targetDistance * (3.0 / 4.0)){
UCHITAKE 1:aed2cca33061 48 //dec dec
UCHITAKE 1:aed2cca33061 49 vel = pow(position - targetDistance, 2) * maxJerk;
UCHITAKE 1:aed2cca33061 50 }
UCHITAKE 1:aed2cca33061 51 }
UCHITAKE 1:aed2cca33061 52
UCHITAKE 1:aed2cca33061 53 return vel;
UCHITAKE 1:aed2cca33061 54 }
UCHITAKE 1:aed2cca33061 55
UCHITAKE 1:aed2cca33061 56 void PositionController::compute(int positionX, int positionY)
UCHITAKE 1:aed2cca33061 57 {
UCHITAKE 1:aed2cca33061 58 velocity[0] = generateVelocity(((double)positionX / (double)targetDistanceX) * targetDistance) * cos(targetRadians);
UCHITAKE 1:aed2cca33061 59 velocity[1] = generateVelocity(((double)positionY / (double)targetDistanceY) * targetDistance) * sin(targetRadians);
UCHITAKE 1:aed2cca33061 60 }
UCHITAKE 1:aed2cca33061 61
UCHITAKE 1:aed2cca33061 62 float PositionController::getVelocityX()
UCHITAKE 1:aed2cca33061 63 {
UCHITAKE 1:aed2cca33061 64 return velocity[0];
UCHITAKE 1:aed2cca33061 65 }
UCHITAKE 1:aed2cca33061 66
UCHITAKE 1:aed2cca33061 67 float PositionController::getVelocityY()
UCHITAKE 1:aed2cca33061 68 {
UCHITAKE 1:aed2cca33061 69 return velocity[1];
UCHITAKE 1:aed2cca33061 70 }