HipControl.h

00001 #ifndef HIPCONTROL_H
00002 #define HIPCONTROL_H
00003 
00004 /**
00005 * Copyright (c) 2012-2014
00006 * All rights reserved.
00007 *
00008  *
00009  * by Bradley Perry
00010  *
00011 */
00012 
00013 
00014 /**
00015 * Control strategys for Daniel's Device
00016 *
00017 * @file control.h
00018 * @author Bradley Perry
00019 *
00020 * @brief Control algorithms
00021 */
00022 
00023 //TODO: (Brad) Port to base-class structure
00024 
00025 #include "mbed.h"
00026 #include "filter.h"
00027 
00028 class HipControl
00029 {
00030 public:
00031     HipControl(PinName pwm, PinName dirpin);
00032 
00033     /**
00034     * Feedback linearization and gain scheduling controller.  Used for all hip trajectory following.
00035     * @param ref Reference point to track.
00036     * @param pos Current position in degrees
00037     * @param Kp Proportional gain
00038     * @param Kd Derivative gain
00039     * @param sat Commanded current saturation
00040     */
00041     //class
00042     void FL(float ref, float pos);
00043 
00044     /**
00045     * Vanilla PD controller for set-point tracking.  Mostly used for haptics.
00046     * @param ref Reference point to track.
00047     * @param pos Current position in degrees
00048     * @param Kp Proportional gain
00049     * @param Kd Derivative gain
00050     * @param sat Commanded current saturation
00051     */
00052     //class
00053     void PD(float ref, float pos);
00054 //class
00055     void P(float ref, float pos);
00056     //base method
00057     void setGains(float P, float D);
00058     //base method
00059     void setSat(float limit);
00060     //base method
00061     void sampleTime(float time);
00062     //class
00063     void openLoop(float input);
00064     //base method
00065     float readPWM();
00066     //class
00067     void off();
00068     //base method
00069     void flip();
00070     //base method
00071     void clear();
00072     //base method
00073     void pwmPeriod(float a);
00074 private:
00075     //Controller Parameters
00076     //const float Kp=.05;
00077     PwmOut _pwm;
00078     DigitalOut _dir;
00079     float Kp;
00080     /**
00081     * Initial proportional gain before cosine gain schedule
00082     */
00083     const float Kp0;
00084     /**
00085     * Derivative gain
00086     */
00087     float Kd;
00088     /**
00089     * Commanded current saturation
00090     */
00091     float sat;
00092     float u;
00093     float u_prev;
00094     float error[2];
00095     //sample period
00096     float _sample_period;
00097     int sign;
00098     filter controlFilter;
00099 };
00100 
00101 //Controller Parameters
00102 
00103 
00104 /**
00105 * Counter for proportional gain cosine gain scheduling
00106 */
00107 /**
00108 * Vector to store error data
00109 */
00110 
00111 /**
00112 * Cosine magnitude for gain scheduling
00113 */
00114 /**
00115 * Cosine frequency for gain scheduling
00116 */
00117 /**
00118 * Offset for gain scheduling
00119 */
00120 
00121 #endif
00122 
00123 
00124