00001 #include "HeadPointerMC.h"
00002 #include "Shared/debuget.h"
00003 #include "Shared/WorldState.h"
00004 #include "MotionManager.h"
00005 #include <math.h>
00006
00007 const OutputCmd HeadPointerMC::unused;
00008
00009 HeadPointerMC::HeadPointerMC() :
00010 MotionCommand(), dirty(true), active(true) {
00011 setWeight(1);
00012 for(unsigned int i=0; i<NumHeadJoints; i++) {
00013 headModes[i]=BodyRelative;
00014 headValues[i]=0;
00015 }
00016 }
00017
00018 void HeadPointerMC::setJoints(double tilt, double pan, double roll) {
00019 headValues[TiltOffset]=tilt;
00020 headValues[PanOffset]=pan;
00021 headValues[RollOffset]=roll;
00022 dirty=true;
00023 }
00024
00025 void HeadPointerMC::setWeight(double w) {
00026 for(unsigned int x=0; x<NumHeadJoints; x++)
00027 headJoints[x].weight=w;
00028 dirty=true;
00029 }
00030
00031 void HeadPointerMC::setMode(CoordFrame_t m, bool conv) {
00032 for(unsigned int x=0; x<NumHeadJoints; x++)
00033 setJointMode((RobotInfo::TPROffset_t)x,m,conv);
00034 }
00035
00036 void HeadPointerMC::setJointMode(RobotInfo::TPROffset_t i, CoordFrame_t m, bool conv) {
00037 if(conv)
00038 headValues[i]=HeadPointerMC::convert(i,headValues[i],headModes[i],m);
00039 headModes[i]=m;
00040 dirty=true;
00041 }
00042
00043 int HeadPointerMC::updateOutputs() {
00044 int tmp=isDirty();
00045 if(tmp) {
00046 dirty=false;
00047 for(unsigned int i=0; i<NumHeadJoints; i++) {
00048 if(headModes[i]!=BodyRelative) {
00049 headJoints[i].value=convToBodyRelative((RobotInfo::TPROffset_t)i,headValues[i],headModes[i]);
00050 dirty=true;
00051 } else
00052 headJoints[i].value=headValues[i];
00053 for(unsigned int f=0; f<NumFrames; f++)
00054 motman->setOutput(this,i+HeadOffset,headJoints[i]);
00055 }
00056 }
00057 return tmp;
00058 }
00059
00060 const OutputCmd& HeadPointerMC::getOutputCmd(unsigned int i) {
00061 i-=HeadOffset;
00062 if(i<NumHeadJoints && getActive())
00063 return headJoints[i];
00064 else
00065 return unused;
00066 }
00067
00068
00069 double HeadPointerMC::convToBodyRelative(RobotInfo::TPROffset_t i, double v, CoordFrame_t mode) const {
00070 switch(mode) {
00071 default:
00072 ASSERT(false,"Passed bad mode "<<mode);
00073 case BodyRelative:
00074 return v;
00075 case GravityRelative: {
00076 double bacc=state->sensors[BAccelOffset];
00077 double lacc=state->sensors[LAccelOffset];
00078 double dacc=state->sensors[DAccelOffset];
00079 switch(i) {
00080 case TiltOffset:
00081 return v+atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00082 case PanOffset:
00083 return v;
00084 case RollOffset:
00085 return v+atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00086 default:
00087 ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00088 return v;
00089 }
00090 }
00091 }
00092 }
00093
00094
00095 double HeadPointerMC::convFromBodyRelative(RobotInfo::TPROffset_t i, double v, CoordFrame_t mode) const {
00096 switch(mode) {
00097 default:
00098 ASSERT(false,"Passed bad mode "<<mode);
00099 case BodyRelative:
00100 return v;
00101 case GravityRelative: {
00102 double bacc=state->sensors[BAccelOffset];
00103 double lacc=state->sensors[LAccelOffset];
00104 double dacc=state->sensors[DAccelOffset];
00105 switch(i) {
00106 case TiltOffset:
00107 return v-atan2(bacc,sqrt(dacc*dacc+lacc*lacc));
00108 case PanOffset:
00109 return v;
00110 case RollOffset:
00111 return v-atan2(-lacc,sqrt(dacc*dacc+bacc*bacc));
00112 default:
00113 ASSERT(false,"Passed bad offset "<<i<<" (use RobotInfo::TPROffset_t!)");
00114 return v;
00115 }
00116 }
00117 }
00118 }
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132