Control function for hip motors
HipControl.h@0:911517b34248, 2014-11-19 (annotated)
- Committer:
- perr1940
- Date:
- Wed Nov 19 22:11:34 2014 +0000
- Revision:
- 0:911517b34248
- Child:
- 1:d87dac5c3658
Sharing with Michael Ling
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
perr1940 | 0:911517b34248 | 1 | |
perr1940 | 0:911517b34248 | 2 | |
perr1940 | 0:911517b34248 | 3 | #ifndef HIPCONTROL_H |
perr1940 | 0:911517b34248 | 4 | #define HIPCONTROL_H |
perr1940 | 0:911517b34248 | 5 | |
perr1940 | 0:911517b34248 | 6 | /** |
perr1940 | 0:911517b34248 | 7 | * Copyright (c) 2012-2014 |
perr1940 | 0:911517b34248 | 8 | * All rights reserved. |
perr1940 | 0:911517b34248 | 9 | * |
perr1940 | 0:911517b34248 | 10 | * |
perr1940 | 0:911517b34248 | 11 | * by Bradley Perry |
perr1940 | 0:911517b34248 | 12 | * |
perr1940 | 0:911517b34248 | 13 | */ |
perr1940 | 0:911517b34248 | 14 | |
perr1940 | 0:911517b34248 | 15 | |
perr1940 | 0:911517b34248 | 16 | /** |
perr1940 | 0:911517b34248 | 17 | * Control strategys for Daniel's Device |
perr1940 | 0:911517b34248 | 18 | * |
perr1940 | 0:911517b34248 | 19 | * @file control.h |
perr1940 | 0:911517b34248 | 20 | * @author Bradley Perry |
perr1940 | 0:911517b34248 | 21 | * |
perr1940 | 0:911517b34248 | 22 | * @brief Control algorithms |
perr1940 | 0:911517b34248 | 23 | */ |
perr1940 | 0:911517b34248 | 24 | |
perr1940 | 0:911517b34248 | 25 | #include "mbed.h" |
perr1940 | 0:911517b34248 | 26 | #include "filter.h" |
perr1940 | 0:911517b34248 | 27 | |
perr1940 | 0:911517b34248 | 28 | class HipControl |
perr1940 | 0:911517b34248 | 29 | { |
perr1940 | 0:911517b34248 | 30 | public: |
perr1940 | 0:911517b34248 | 31 | HipControl(PinName pwm, PinName dirpin); |
perr1940 | 0:911517b34248 | 32 | void FL(float ref, float pos); |
perr1940 | 0:911517b34248 | 33 | void FL_new(float ref, float pos); |
perr1940 | 0:911517b34248 | 34 | void PD(float ref, float pos); |
perr1940 | 0:911517b34248 | 35 | void P(float ref, float pos); |
perr1940 | 0:911517b34248 | 36 | void setGains(float P, float D); |
perr1940 | 0:911517b34248 | 37 | void setSat(float limit); |
perr1940 | 0:911517b34248 | 38 | void sampleTime(float time); |
perr1940 | 0:911517b34248 | 39 | void openLoop(float input); |
perr1940 | 0:911517b34248 | 40 | float read(); |
perr1940 | 0:911517b34248 | 41 | void off(); |
perr1940 | 0:911517b34248 | 42 | void flip(); |
perr1940 | 0:911517b34248 | 43 | void clear(); |
perr1940 | 0:911517b34248 | 44 | void pwmPeriod(float a); |
perr1940 | 0:911517b34248 | 45 | private: |
perr1940 | 0:911517b34248 | 46 | //Controller Parameters |
perr1940 | 0:911517b34248 | 47 | //const float Kp=.05; |
perr1940 | 0:911517b34248 | 48 | PwmOut _pwm; |
perr1940 | 0:911517b34248 | 49 | DigitalOut _dir; |
perr1940 | 0:911517b34248 | 50 | float Kp; |
perr1940 | 0:911517b34248 | 51 | const float Kp0; |
perr1940 | 0:911517b34248 | 52 | float Kd; |
perr1940 | 0:911517b34248 | 53 | float sat; |
perr1940 | 0:911517b34248 | 54 | float u; |
perr1940 | 0:911517b34248 | 55 | float u_prev; |
perr1940 | 0:911517b34248 | 56 | float error[2]; |
perr1940 | 0:911517b34248 | 57 | float T; |
perr1940 | 0:911517b34248 | 58 | int sign; |
perr1940 | 0:911517b34248 | 59 | filter controlFilter; |
perr1940 | 0:911517b34248 | 60 | }; |
perr1940 | 0:911517b34248 | 61 | |
perr1940 | 0:911517b34248 | 62 | //Controller Parameters |
perr1940 | 0:911517b34248 | 63 | /** |
perr1940 | 0:911517b34248 | 64 | * Proportional gain after cosine gain schedule |
perr1940 | 0:911517b34248 | 65 | */ |
perr1940 | 0:911517b34248 | 66 | /** |
perr1940 | 0:911517b34248 | 67 | * Initial proportional gain before cosine gain schedule |
perr1940 | 0:911517b34248 | 68 | */ |
perr1940 | 0:911517b34248 | 69 | /** |
perr1940 | 0:911517b34248 | 70 | * Derivative gain |
perr1940 | 0:911517b34248 | 71 | */ |
perr1940 | 0:911517b34248 | 72 | |
perr1940 | 0:911517b34248 | 73 | /** |
perr1940 | 0:911517b34248 | 74 | * Commanded current soft stop |
perr1940 | 0:911517b34248 | 75 | */ |
perr1940 | 0:911517b34248 | 76 | /** |
perr1940 | 0:911517b34248 | 77 | * Counter for proportional gain cosine gain scheduling |
perr1940 | 0:911517b34248 | 78 | */ |
perr1940 | 0:911517b34248 | 79 | /** |
perr1940 | 0:911517b34248 | 80 | * Vector to store error data |
perr1940 | 0:911517b34248 | 81 | */ |
perr1940 | 0:911517b34248 | 82 | |
perr1940 | 0:911517b34248 | 83 | /** |
perr1940 | 0:911517b34248 | 84 | * Cosine magnitude for gain scheduling |
perr1940 | 0:911517b34248 | 85 | */ |
perr1940 | 0:911517b34248 | 86 | /** |
perr1940 | 0:911517b34248 | 87 | * Cosine frequency for gain scheduling |
perr1940 | 0:911517b34248 | 88 | */ |
perr1940 | 0:911517b34248 | 89 | /** |
perr1940 | 0:911517b34248 | 90 | * Offset for gain scheduling |
perr1940 | 0:911517b34248 | 91 | */ |
perr1940 | 0:911517b34248 | 92 | |
perr1940 | 0:911517b34248 | 93 | #endif |
perr1940 | 0:911517b34248 | 94 | |
perr1940 | 0:911517b34248 | 95 | /** |
perr1940 | 0:911517b34248 | 96 | * Feedback linearization and gain scheduling controller. Used for all hip trajectory following. |
perr1940 | 0:911517b34248 | 97 | * @param ref Reference point to track. |
perr1940 | 0:911517b34248 | 98 | * @param pos Current position in degrees |
perr1940 | 0:911517b34248 | 99 | * @param Kp Proportional gain |
perr1940 | 0:911517b34248 | 100 | * @param Kd Derivative gain |
perr1940 | 0:911517b34248 | 101 | * @param sat Commanded current saturation |
perr1940 | 0:911517b34248 | 102 | */ |
perr1940 | 0:911517b34248 | 103 | |
perr1940 | 0:911517b34248 | 104 | /** |
perr1940 | 0:911517b34248 | 105 | * Vanilla PD controller for set-point tracking. Mostly used for haptics. |
perr1940 | 0:911517b34248 | 106 | * @param ref Reference point to track. |
perr1940 | 0:911517b34248 | 107 | * @param pos Current position in degrees |
perr1940 | 0:911517b34248 | 108 | * @param Kp Proportional gain |
perr1940 | 0:911517b34248 | 109 | * @param Kd Derivative gain |
perr1940 | 0:911517b34248 | 110 | * @param sat Commanded current saturation |
perr1940 | 0:911517b34248 | 111 | */ |