Bradley Perry / HipControl

Control function for hip motors

HipControl.h@1:d87dac5c3658, 2015-06-24 (annotated)

Committer:
perr1940
Date:
Wed Jun 24 01:07:55 2015 +0000
Revision:
1:d87dac5c3658
Parent:
0:911517b34248 
I don't know what I changed

Who changed what in which revision?

UserRevisionLine numberNew 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 1:d87dac5c3658 25 //TODO: (Brad) Port to base-class structure
perr1940 1:d87dac5c3658 26
perr1940 0:911517b34248 27 #include "mbed.h"
perr1940 0:911517b34248 28 #include "filter.h"
perr1940 0:911517b34248 29
perr1940 0:911517b34248 30 class HipControl
perr1940 0:911517b34248 31 {
perr1940 0:911517b34248 32 public:
perr1940 0:911517b34248 33 HipControl(PinName pwm, PinName dirpin);
perr1940 1:d87dac5c3658 34
perr1940 1:d87dac5c3658 35 /**
perr1940 1:d87dac5c3658 36 * Feedback linearization and gain scheduling controller. Used for all hip trajectory following.
perr1940 1:d87dac5c3658 37 * @param ref Reference point to track.
perr1940 1:d87dac5c3658 38 * @param pos Current position in degrees
perr1940 1:d87dac5c3658 39 * @param Kp Proportional gain
perr1940 1:d87dac5c3658 40 * @param Kd Derivative gain
perr1940 1:d87dac5c3658 41 * @param sat Commanded current saturation
perr1940 1:d87dac5c3658 42 */
perr1940 1:d87dac5c3658 43 //class
perr1940 0:911517b34248 44 void FL(float ref, float pos);
perr1940 1:d87dac5c3658 45
perr1940 1:d87dac5c3658 46 /**
perr1940 1:d87dac5c3658 47 * Vanilla PD controller for set-point tracking. Mostly used for haptics.
perr1940 1:d87dac5c3658 48 * @param ref Reference point to track.
perr1940 1:d87dac5c3658 49 * @param pos Current position in degrees
perr1940 1:d87dac5c3658 50 * @param Kp Proportional gain
perr1940 1:d87dac5c3658 51 * @param Kd Derivative gain
perr1940 1:d87dac5c3658 52 * @param sat Commanded current saturation
perr1940 1:d87dac5c3658 53 */
perr1940 1:d87dac5c3658 54 //class
perr1940 0:911517b34248 55 void PD(float ref, float pos);
perr1940 1:d87dac5c3658 56 //class
perr1940 0:911517b34248 57 void P(float ref, float pos);
perr1940 1:d87dac5c3658 58 //base method
perr1940 0:911517b34248 59 void setGains(float P, float D);
perr1940 1:d87dac5c3658 60 //base method
perr1940 0:911517b34248 61 void setSat(float limit);
perr1940 1:d87dac5c3658 62 //base method
perr1940 0:911517b34248 63 void sampleTime(float time);
perr1940 1:d87dac5c3658 64 //class
perr1940 0:911517b34248 65 void openLoop(float input);
perr1940 1:d87dac5c3658 66 //base method
perr1940 1:d87dac5c3658 67 float readPWM();
perr1940 1:d87dac5c3658 68 //class
perr1940 0:911517b34248 69 void off();
perr1940 1:d87dac5c3658 70 //base method
perr1940 0:911517b34248 71 void flip();
perr1940 1:d87dac5c3658 72 //base method
perr1940 0:911517b34248 73 void clear();
perr1940 1:d87dac5c3658 74 //base method
perr1940 0:911517b34248 75 void pwmPeriod(float a);
perr1940 0:911517b34248 76 private:
perr1940 0:911517b34248 77 //Controller Parameters
perr1940 0:911517b34248 78 //const float Kp=.05;
perr1940 0:911517b34248 79 PwmOut _pwm;
perr1940 0:911517b34248 80 DigitalOut _dir;
perr1940 0:911517b34248 81 float Kp;
perr1940 1:d87dac5c3658 82 /**
perr1940 1:d87dac5c3658 83 * Initial proportional gain before cosine gain schedule
perr1940 1:d87dac5c3658 84 */
perr1940 0:911517b34248 85 const float Kp0;
perr1940 1:d87dac5c3658 86 /**
perr1940 1:d87dac5c3658 87 * Derivative gain
perr1940 1:d87dac5c3658 88 */
perr1940 0:911517b34248 89 float Kd;
perr1940 1:d87dac5c3658 90 /**
perr1940 1:d87dac5c3658 91 * Commanded current saturation
perr1940 1:d87dac5c3658 92 */
perr1940 0:911517b34248 93 float sat;
perr1940 0:911517b34248 94 float u;
perr1940 0:911517b34248 95 float u_prev;
perr1940 0:911517b34248 96 float error[2];
perr1940 1:d87dac5c3658 97 //sample period
perr1940 1:d87dac5c3658 98 float _sample_period;
perr1940 0:911517b34248 99 int sign;
perr1940 0:911517b34248 100 filter controlFilter;
perr1940 0:911517b34248 101 };
perr1940 0:911517b34248 102
perr1940 0:911517b34248 103 //Controller Parameters
perr1940 0:911517b34248 104
perr1940 1:d87dac5c3658 105
perr1940 0:911517b34248 106 /**
perr1940 0:911517b34248 107 * Counter for proportional gain cosine gain scheduling
perr1940 0:911517b34248 108 */
perr1940 0:911517b34248 109 /**
perr1940 0:911517b34248 110 * Vector to store error data
perr1940 0:911517b34248 111 */
perr1940 0:911517b34248 112
perr1940 0:911517b34248 113 /**
perr1940 0:911517b34248 114 * Cosine magnitude for gain scheduling
perr1940 0:911517b34248 115 */
perr1940 0:911517b34248 116 /**
perr1940 0:911517b34248 117 * Cosine frequency for gain scheduling
perr1940 0:911517b34248 118 */
perr1940 0:911517b34248 119 /**
perr1940 0:911517b34248 120 * Offset for gain scheduling
perr1940 0:911517b34248 121 */
perr1940 0:911517b34248 122
perr1940 0:911517b34248 123 #endif
perr1940 0:911517b34248 124
perr1940 0:911517b34248 125
perr1940 1:d87dac5c3658 126