00001
00002 #ifndef INCLUDED_PIDMC_h_
00003 #define INCLUDED_PIDMC_h_
00004
00005 #include "MotionCommand.h"
00006 #include "MotionManager.h"
00007 #include "OutputPID.h"
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 class PIDMC : public MotionCommand {
00042 public:
00043
00044 PIDMC() : MotionCommand(), dirty(false) {
00045 setDefaults(0);
00046 dirty=false;
00047 }
00048
00049 PIDMC(float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00050 setAllPowerLevel(powerlevel,w);
00051 }
00052
00053 PIDMC(unsigned int low, unsigned int high, float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00054 setRangePowerLevel(PIDJointOffset,low,1.f,0.f);
00055 setRangePowerLevel(low,high,powerlevel,w);
00056 setRangePowerLevel(high,PIDJointOffset+NumPIDJoints,1.f,0.f);
00057 }
00058
00059 virtual ~PIDMC() {}
00060
00061
00062
00063 virtual int updateOutputs() {
00064 for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00065 motman->setOutput(this,i+PIDJointOffset,PIDs[i]);
00066 int wasDirty=dirty; dirty=false; return wasDirty;
00067 }
00068 virtual int isDirty() { return dirty; }
00069 virtual int isAlive() { return dirty; }
00070 virtual void DoStart() { MotionCommand::DoStart(); dirty=true; }
00071
00072
00073
00074 void setDefaults(float weight=1) {
00075 setAllPowerLevel(1.f,weight);
00076 }
00077
00078
00079 void setJointPowerLevel(unsigned int i, float p, float w=1) {
00080 i-=PIDJointOffset;
00081 for(unsigned int j=0;j<3;j++)
00082 PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00083 PIDs[i].weight=w;
00084 dirty=true;
00085 }
00086
00087
00088 void setAllPowerLevel(float p, float w=1) {
00089 for(unsigned int i=0; i<NumPIDJoints; i++) {
00090 for(unsigned int j=0;j<3;j++)
00091 PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00092 PIDs[i].weight=w;
00093 }
00094 dirty=true;
00095 }
00096
00097
00098 void setRangePowerLevel(unsigned int low, unsigned int high, float p, float w=1) {
00099 low-=PIDJointOffset;
00100 high-=PIDJointOffset;
00101 for(unsigned int i=low; i<high; i++) {
00102 for(unsigned int j=0;j<3;j++)
00103 PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00104 PIDs[i].weight=w;
00105 }
00106 dirty=true;
00107 }
00108
00109
00110
00111 void setPID(unsigned int i, const OutputPID& pid) {
00112 i-=PIDJointOffset;
00113 PIDs[i]=pid;
00114 dirty=true;
00115 }
00116
00117
00118 OutputPID& getPID(unsigned int i) {
00119 return PIDs[i-PIDJointOffset];
00120 }
00121
00122
00123 const OutputPID& getPID(unsigned int i) const {
00124 return PIDs[i-PIDJointOffset];
00125 }
00126
00127 protected:
00128
00129 static inline bool isPID(unsigned int i) {
00130 return ((int)i>=(int)PIDJointOffset && i<PIDJointOffset+NumPIDJoints);
00131 }
00132
00133 bool dirty;
00134 OutputPID PIDs[NumPIDJoints];
00135 };
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148 #endif