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 class PIDMC : public MotionCommand {
00012 public:
00013
00014 PIDMC() : MotionCommand(), dirty(false) {
00015 setDefaults(0);
00016 dirty=false;
00017 }
00018
00019 PIDMC(float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00020 setAllPowerLevel(powerlevel,w);
00021 }
00022
00023 PIDMC(unsigned int low, unsigned int high, float powerlevel, float w=1) : MotionCommand(), dirty(false) {
00024 setRangePowerLevel(PIDJointOffset,low,1.f,0.f);
00025 setRangePowerLevel(low,high,powerlevel,w);
00026 setRangePowerLevel(high,PIDJointOffset+NumPIDJoints,1.f,0.f);
00027 }
00028
00029 virtual ~PIDMC() {}
00030
00031
00032
00033 virtual int updateOutputs() {
00034 for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00035 motman->setOutput(this,i+PIDJointOffset,PIDs[i]);
00036 int wasDirty=dirty; dirty=false; return wasDirty;
00037 }
00038 virtual int isDirty() { return dirty; }
00039 virtual int isAlive() { return dirty; }
00040
00041
00042
00043 void setDefaults(float weight=1) {
00044 setAllPowerLevel(1.f,weight);
00045 }
00046
00047
00048 void setJointPowerLevel(unsigned int i, float p, float w=1) {
00049 i-=PIDJointOffset;
00050 for(unsigned int j=0;j<3;j++)
00051 PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00052 PIDs[i].weight=w;
00053 dirty=true;
00054 }
00055
00056
00057 void setAllPowerLevel(float p, float w=1) {
00058 for(unsigned int i=0; i<NumPIDJoints; i++) {
00059 for(unsigned int j=0;j<3;j++)
00060 PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00061 PIDs[i].weight=w;
00062 }
00063 dirty=true;
00064 }
00065
00066
00067 void setRangePowerLevel(unsigned int low, unsigned int high, float p, float w=1) {
00068 low-=PIDJointOffset;
00069 high-=PIDJointOffset;
00070 for(unsigned int i=low; i<high; i++) {
00071 for(unsigned int j=0;j<3;j++)
00072 PIDs[i].pid[j]=DefaultPIDs[i][j]*p;
00073 PIDs[i].weight=w;
00074 }
00075 dirty=true;
00076 }
00077
00078
00079
00080 void setPID(unsigned int i, const OutputPID& pid) {
00081 i-=PIDJointOffset;
00082 PIDs[i]=pid;
00083 dirty=true;
00084 }
00085
00086
00087 OutputPID& getPID(unsigned int i) {
00088 return PIDs[i-PIDJointOffset];
00089 }
00090
00091
00092 const OutputPID& getPID(unsigned int i) const {
00093 return PIDs[i-PIDJointOffset];
00094 }
00095
00096 protected:
00097
00098 static inline bool isPID(unsigned int i) {
00099 return ((int)i>=(int)PIDJointOffset && i<PIDJointOffset+NumPIDJoints);
00100 }
00101
00102 bool dirty;
00103 OutputPID PIDs[NumPIDJoints];
00104 };
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 #endif