amiro-os / include / amiro / DistControl.h @ a3c54343
History | View | Annotate | Download (2.175 KB)
| 1 | 58fe0e0b | Thomas Schöpping | #ifndef AMIRO_MOTOR_DISTCONTROL_H_
|
|---|---|---|---|
| 2 | #define AMIRO_MOTOR_DISTCONTROL_H_
|
||
| 3 | |||
| 4 | #include <amiro/MotorIncrements.h> |
||
| 5 | #include <amiro/MotorControl.h> |
||
| 6 | #include <amiro/Constants.h> |
||
| 7 | #include <Types.h> |
||
| 8 | |||
| 9 | #include <chprintf.h> |
||
| 10 | |||
| 11 | namespace amiro {
|
||
| 12 | |||
| 13 | class DistControl : public chibios_rt::BaseStaticThread<256> {
|
||
| 14 | public:
|
||
| 15 | /**
|
||
| 16 | * Constructor
|
||
| 17 | *
|
||
| 18 | * @param mc object for retrieving the motors
|
||
| 19 | * @param mi object for retrieving the motor increments of the qei
|
||
| 20 | */
|
||
| 21 | DistControl(MotorControl* mc, MotorIncrements* mi); |
||
| 22 | |||
| 23 | /**
|
||
| 24 | * Get the current target distance in um.
|
||
| 25 | *
|
||
| 26 | * @return target distance in um
|
||
| 27 | */
|
||
| 28 | int getCurrentTargetDist(void); |
||
| 29 | |||
| 30 | /**
|
||
| 31 | * Get the current target angle in urad.
|
||
| 32 | *
|
||
| 33 | * @return target distance in urad
|
||
| 34 | */
|
||
| 35 | int getCurrentTargetAngle(void); |
||
| 36 | |||
| 37 | /**
|
||
| 38 | * Sets the target position.
|
||
| 39 | *
|
||
| 40 | * @param distance Distance to drive in um
|
||
| 41 | * @param angle Angle to turn in urad
|
||
| 42 | * @param time Given time to drive in ms
|
||
| 43 | */
|
||
| 44 | void setTargetPosition(int32_t distance, int32_t angle, uint16_t time);
|
||
| 45 | |||
| 46 | /**
|
||
| 47 | * Gives if the distance controller is active.
|
||
| 48 | *
|
||
| 49 | * @return true if active, false if inactive
|
||
| 50 | */
|
||
| 51 | bool isActive(void); |
||
| 52 | |||
| 53 | /**
|
||
| 54 | * Deactivates the controller.
|
||
| 55 | * The motor velocities won't be set in any way!
|
||
| 56 | */
|
||
| 57 | void deactivateController(void); |
||
| 58 | |||
| 59 | |||
| 60 | |||
| 61 | protected:
|
||
| 62 | virtual msg_t main(void);
|
||
| 63 | |||
| 64 | private:
|
||
| 65 | /**
|
||
| 66 | * Calculates the velocities by using the error distance and error angle.
|
||
| 67 | */
|
||
| 68 | void calcVelocities(void); |
||
| 69 | |||
| 70 | MotorControl* motorControl; |
||
| 71 | MotorIncrements* motorIncrements; |
||
| 72 | bool controllerActive, drivingForward, turningLeft, newVelocities;
|
||
| 73 | const uint32_t period;
|
||
| 74 | int32_t increment[2];
|
||
| 75 | float incrementDifference[2]; |
||
| 76 | float actualDistance[2]; // m |
||
| 77 | int32_t fullDistance[2]; // um |
||
| 78 | int32_t realDistance; // um
|
||
| 79 | int32_t realAngle; // urad
|
||
| 80 | int32_t targetDistance; // um
|
||
| 81 | int32_t targetAngle; // urad
|
||
| 82 | int32_t errorDistance; // um
|
||
| 83 | int32_t errorAngle; // urad
|
||
| 84 | uint32_t restTime; // us
|
||
| 85 | types::kinematic maxVelocity; |
||
| 86 | types::kinematic targetVelocity; |
||
| 87 | types::kinematic minVelocity; |
||
| 88 | }; |
||
| 89 | } |
||
| 90 | |||
| 91 | #endif /* AMIRO_MOTOR_DISTCONTROL_H_ */ |