HipControl.h

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