Tekkotsu Homepage

Demos

Overview

Downloads

Dev. Resources

Reference

Credits

WorldState.cc Go to the documentation of this file. #include "WorldState.h" #include "Shared/get_time.h" #include "Events/EventRouter.h" #include "ERS210Info.h" #include "ERS220Info.h" #include "ERS7Info.h" #include "Shared/Config.h" #include "IPC/ProcessID.h" #ifdef PLATFORM_APERIOS # include <OPENR/core_macro.h> # include <OPENR/ObjcommTypes.h> # include <OPENR/OPENR.h> # include <OPENR/OPENRAPI.h> # include <OPENR/OPENRMessages.h> # include <OPENR/OPower.h> #else # include "Motion/PostureEngine.h" #endif #define GETD(cpc) (((float)sensor.GetData(cpc)->frame[lastFrame].value) / 1.0E6f) //!< returns value from OPEN-R, converted from micro in int to base in float #define GETB(cpc) ((bool)sensor.GetData(cpc)->frame[lastFrame].value) //!< returns value from OPEN-R, as bool #define GETSENSOR(cpc) ((float)sensor.GetData(cpc)->frame[lastFrame].value) //!< return value from OPEN-R, as int #define GETSIG(cpc) ((word)sensor.GetData(cpc)->frame[lastFrame].signal) //!< returns signal from OPEN-R as word #define GETDUTY(cpc) ((float)((OJointValue*)&sensor.GetData(cpc)->frame[lastFrame])->pwmDuty/512.0f) //!< returns duty cycle from OPEN-R as float; -1 (full reverse) to 0 (idle) to 1 (full forward) const double WorldState::g=9.80665; const double WorldState::IROORDist = 900.0; #ifdef PLATFORM_APERIOS WorldState ** WorldState::stateLookupMap=NULL; #endif WorldState * state=NULL; WorldState::WorldState() : alwaysGenerateStatus(false), vel_x(0), vel_y(0), vel_a(0), vel_time(0), robotStatus(0), batteryStatus(0), lastSensorUpdateTime(0), frameNumber(1), framesProcessed(0), robotDesign(0), curtime(0) { for(unsigned int i=0; i< NumOutputs; i++) outputs[i]=0; for(unsigned int i=0; i< NumButtons; i++) buttons[i]=0; for(unsigned int i=0; i< NumSensors; i++) sensors[i]=0; for(unsigned int i=0; i< NumPIDJoints; i++) for(unsigned int j=0; j<3; j++) pids[i][j]=0; for(unsigned int i=0; i< NumPIDJoints; i++) pidduties[i]=0; memset(powerFlags,0,sizeof(unsigned int)*PowerSourceID::NumPowerSIDs); memset(button_times,0,sizeof(unsigned int)*NumButtons); #ifdef PLATFORM_APERIOS //Thanks Daishi: char robotDesignStr[orobotdesignNAME_MAX + 1]; memset(robotDesignStr, 0, sizeof(robotDesignStr)); if (OPENR::GetRobotDesign(robotDesignStr) != oSUCCESS) { cout << "OPENR::GetRobotDesign() failed." << endl; } else { if(strcmp(robotDesignStr,"ERS-210")==0) robotDesign=ERS210Mask; else if(strcmp(robotDesignStr,"ERS-220")==0) robotDesign=ERS220Mask; else if(strcmp(robotDesignStr,"ERS-7")==0) robotDesign=ERS7Mask; else { cout << "ERROR: Unrecognized model: "<<robotDesignStr<<"\nDoing the best I can by assuming ERS-7..."<<endl; robotDesign=ERS7Mask; } } #else //This is only compiled if we're targeting the local platform //In other words, if target model is an AIBO, simulation mode # if TGT_ERS220 robotDesign=ERS220Mask; # elif TGT_ERS210 robotDesign=ERS210Mask; # elif TGT_ERS2xx # warning "TGT_ERS2xx can't be determined on non-Aperios - defaulting to TGT_ERS210" robotDesign=ERS210Mask; # elif TGT_ERS7 robotDesign=ERS7Mask; # else # warning "TGT_<model> undefined - defaulting to TGT_ERS7" robotDesign=ERS7Mask; # endif //model selection #endif } #ifdef PLATFORM_APERIOS /*! This will cause events to be posted */ void WorldState::read(OSensorFrameVectorData& sensor, WorldState* lastState, EventRouter* er) { //always using GetInfo(0) to get "global" information for the vector, other infos contain metadata for individual data fields unsigned int newFrameNumber=sensor.GetInfo(0)->frameNumber; if(frameNumber>=newFrameNumber) return; //sensors have already been filled in curtime=get_time(); std::vector<EventBase> evtBuf; unsigned int lastFrame=sensor.GetInfo(0)->numFrames-1; if(lastState!=NULL && lastState!=this) { alwaysGenerateStatus=lastState->alwaysGenerateStatus; //pid and velocity values will be copied at "completion" robotStatus=lastState->robotStatus; batteryStatus=lastState->batteryStatus; for(unsigned int i=0; i<PowerSourceID::NumPowerSIDs; i++) powerFlags[i]=lastState->powerFlags[i]; //important part -- copy over button_times before calls to chkEvent for(unsigned int i=0; i<NumButtons; i++) button_times[i]=lastState->button_times[i]; } if(robotDesign&ERS210Mask) { outputs[LFrLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointLFRotator); outputs[LFrLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointLFElevator); outputs[LFrLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointLFKnee); pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLFRotator); pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLFElevator); pidduties[LFrLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointLFKnee); outputs[RFrLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointRFRotator); outputs[RFrLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointRFElevator); outputs[RFrLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointRFKnee); pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRFRotator); pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRFElevator); pidduties[RFrLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointRFKnee); outputs[LBkLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointLHRotator); outputs[LBkLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointLHElevator); outputs[LBkLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointLHKnee); pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointLHRotator); pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointLHElevator); pidduties[LBkLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointLHKnee); outputs[RBkLegOffset + RotatorOffset ] = GETD(ERS210Info::CPCJointRHRotator); outputs[RBkLegOffset + ElevatorOffset ] = GETD(ERS210Info::CPCJointRHElevator); outputs[RBkLegOffset + KneeOffset ] = GETD(ERS210Info::CPCJointRHKnee); pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS210Info::CPCJointRHRotator); pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS210Info::CPCJointRHElevator); pidduties[RBkLegOffset + KneeOffset ] = GETDUTY(ERS210Info::CPCJointRHKnee); // Get head tilt,pan,roll joint angles outputs[HeadOffset+TiltOffset] = GETD(ERS210Info::CPCJointNeckTilt); outputs[HeadOffset+PanOffset ] = GETD(ERS210Info::CPCJointNeckPan); outputs[HeadOffset+RollOffset] = GETD(ERS210Info::CPCJointNeckRoll); pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS210Info::CPCJointNeckTilt); pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS210Info::CPCJointNeckPan); pidduties[HeadOffset+RollOffset] = GETDUTY(ERS210Info::CPCJointNeckRoll); outputs[ERS210Info::TailOffset+TiltOffset] = GETD(ERS210Info::CPCJointTailTilt); outputs[ERS210Info::TailOffset+PanOffset] = GETD(ERS210Info::CPCJointTailPan); pidduties[ERS210Info::TailOffset+TiltOffset] = GETDUTY(ERS210Info::CPCJointTailTilt); pidduties[ERS210Info::TailOffset+PanOffset] = GETDUTY(ERS210Info::CPCJointTailPan); outputs[ERS210Info::MouthOffset] = GETD(ERS210Info::CPCJointMouth); pidduties[ERS210Info::MouthOffset] = GETDUTY(ERS210Info::CPCJointMouth); if(lastState!=NULL && lastState!=this) { // Copy over new buttons buttons[ERS210Info::LFrPawOffset]=GETB(ERS210Info::CPCSensorLFPaw); buttons[ERS210Info::RFrPawOffset]=GETB(ERS210Info::CPCSensorRFPaw); buttons[ERS210Info::LBkPawOffset]=GETB(ERS210Info::CPCSensorLHPaw); buttons[ERS210Info::RBkPawOffset]=GETB(ERS210Info::CPCSensorRHPaw); buttons[ERS210Info::ChinButOffset]=GETB(ERS210Info::CPCSensorChinSwitch); buttons[ERS210Info::BackButOffset]=GETB(ERS210Info::CPCSensorBackSwitch); buttons[ERS210Info::HeadFrButOffset]=GETD(ERS210Info::CPCSensorHeadFrontPressure); buttons[ERS210Info::HeadBkButOffset]=GETD(ERS210Info::CPCSensorHeadBackPressure); //! process changes chkEvent(evtBuf,lastState); } else { // Get foot switches chkEvent(evtBuf,ERS210Info::LFrPawOffset,GETB(ERS210Info::CPCSensorLFPaw),buttonNames[ERS210Info::LFrPawOffset]); chkEvent(evtBuf,ERS210Info::RFrPawOffset,GETB(ERS210Info::CPCSensorRFPaw),buttonNames[ERS210Info::RFrPawOffset]); chkEvent(evtBuf,ERS210Info::LBkPawOffset,GETB(ERS210Info::CPCSensorLHPaw),buttonNames[ERS210Info::LBkPawOffset]); chkEvent(evtBuf,ERS210Info::RBkPawOffset,GETB(ERS210Info::CPCSensorRHPaw),buttonNames[ERS210Info::RBkPawOffset]); // Get buttons chkEvent(evtBuf,ERS210Info::ChinButOffset, GETB(ERS210Info::CPCSensorChinSwitch),buttonNames[ERS210Info::ChinButOffset]); chkEvent(evtBuf,ERS210Info::BackButOffset, GETB(ERS210Info::CPCSensorBackSwitch),buttonNames[ERS210Info::BackButOffset]); chkEvent(evtBuf,ERS210Info::HeadFrButOffset,GETD(ERS210Info::CPCSensorHeadFrontPressure),buttonNames[ERS210Info::HeadFrButOffset]); chkEvent(evtBuf,ERS210Info::HeadBkButOffset,GETD(ERS210Info::CPCSensorHeadBackPressure),buttonNames[ERS210Info::HeadBkButOffset]); } // Get IR distance sensor sensors[ERS210Info::IRDistOffset]=GETSENSOR(ERS210Info::CPCSensorPSD) / 1000.0f; // Get acceleration sensors sensors[BAccelOffset] = GETD(ERS210Info::CPCSensorAccelFB); sensors[LAccelOffset] = GETD(ERS210Info::CPCSensorAccelLR); sensors[DAccelOffset] = GETD(ERS210Info::CPCSensorAccelUD); sensors[ERS210Info::ThermoOffset] = GETD(ERS210Info::CPCSensorThermoSensor); } // (ERS-220 only) if(robotDesign&ERS220Mask) { outputs[LFrLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointLFRotator); outputs[LFrLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointLFElevator); outputs[LFrLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointLFKnee); pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLFRotator); pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLFElevator); pidduties[LFrLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointLFKnee); outputs[RFrLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointRFRotator); outputs[RFrLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointRFElevator); outputs[RFrLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointRFKnee); pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRFRotator); pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRFElevator); pidduties[RFrLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointRFKnee); outputs[LBkLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointLHRotator); outputs[LBkLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointLHElevator); outputs[LBkLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointLHKnee); pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointLHRotator); pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointLHElevator); pidduties[LBkLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointLHKnee); outputs[RBkLegOffset + RotatorOffset ] = GETD(ERS220Info::CPCJointRHRotator); outputs[RBkLegOffset + ElevatorOffset ] = GETD(ERS220Info::CPCJointRHElevator); outputs[RBkLegOffset + KneeOffset ] = GETD(ERS220Info::CPCJointRHKnee); pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS220Info::CPCJointRHRotator); pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS220Info::CPCJointRHElevator); pidduties[RBkLegOffset + KneeOffset ] = GETDUTY(ERS220Info::CPCJointRHKnee); // Get head tilt,pan,roll joint angles outputs[HeadOffset+TiltOffset] = GETD(ERS220Info::CPCJointNeckTilt); outputs[HeadOffset+PanOffset ] = GETD(ERS220Info::CPCJointNeckPan); outputs[HeadOffset+RollOffset] = GETD(ERS220Info::CPCJointNeckRoll); pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS220Info::CPCJointNeckTilt); pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS220Info::CPCJointNeckPan); pidduties[HeadOffset+RollOffset] = GETDUTY(ERS220Info::CPCJointNeckRoll); if(lastState!=NULL && lastState!=this) { // Copy over new buttons buttons[ERS220Info::LFrPawOffset]=GETB(ERS220Info::CPCSensorLFPaw); buttons[ERS220Info::RFrPawOffset]=GETB(ERS220Info::CPCSensorRFPaw); buttons[ERS220Info::LBkPawOffset]=GETB(ERS220Info::CPCSensorLHPaw); buttons[ERS220Info::RBkPawOffset]=GETB(ERS220Info::CPCSensorRHPaw); buttons[ERS220Info::ChinButOffset]=GETB(ERS220Info::CPCSensorChinSwitch); buttons[ERS220Info::BackButOffset]=GETB(ERS220Info::CPCSensorBackSwitch); buttons[ERS220Info::HeadFrButOffset]=GETD(ERS220Info::CPCSensorHeadFrontPressure); buttons[ERS220Info::HeadBkButOffset]=GETD(ERS220Info::CPCSensorHeadBackPressure); buttons[ERS220Info::TailLeftButOffset]=GETB(ERS220Info::CPCSensorTailLeftSwitch); buttons[ERS220Info::TailCenterButOffset]=GETB(ERS220Info::CPCSensorTailCenterSwitch); buttons[ERS220Info::TailRightButOffset]=GETB(ERS220Info::CPCSensorTailRightSwitch); //! process changes chkEvent(evtBuf,lastState); } else { // Get foot switches chkEvent(evtBuf,ERS220Info::LFrPawOffset,GETB(ERS220Info::CPCSensorLFPaw),buttonNames[ERS220Info::LFrPawOffset]); chkEvent(evtBuf,ERS220Info::RFrPawOffset,GETB(ERS220Info::CPCSensorRFPaw),buttonNames[ERS220Info::RFrPawOffset]); chkEvent(evtBuf,ERS220Info::LBkPawOffset,GETB(ERS220Info::CPCSensorLHPaw),buttonNames[ERS220Info::LBkPawOffset]); chkEvent(evtBuf,ERS220Info::RBkPawOffset,GETB(ERS220Info::CPCSensorRHPaw),buttonNames[ERS220Info::RBkPawOffset]); // Get buttons chkEvent(evtBuf,ERS220Info::ChinButOffset, GETB(ERS220Info::CPCSensorChinSwitch),buttonNames[ERS220Info::ChinButOffset]); chkEvent(evtBuf,ERS220Info::BackButOffset, GETB(ERS220Info::CPCSensorBackSwitch),buttonNames[ERS220Info::BackButOffset]); chkEvent(evtBuf,ERS220Info::HeadFrButOffset,GETD(ERS220Info::CPCSensorHeadFrontPressure),buttonNames[ERS220Info::HeadFrButOffset]); chkEvent(evtBuf,ERS220Info::HeadBkButOffset,GETD(ERS220Info::CPCSensorHeadBackPressure),buttonNames[ERS220Info::HeadBkButOffset]); chkEvent(evtBuf,ERS220Info::TailLeftButOffset, GETB(ERS220Info::CPCSensorTailLeftSwitch), buttonNames[ERS220Info::TailLeftButOffset]); chkEvent(evtBuf,ERS220Info::TailCenterButOffset, GETB(ERS220Info::CPCSensorTailCenterSwitch),buttonNames[ERS220Info::TailCenterButOffset]); chkEvent(evtBuf,ERS220Info::TailRightButOffset, GETB(ERS220Info::CPCSensorTailRightSwitch), buttonNames[ERS220Info::TailRightButOffset]); } // Get IR distance sensor sensors[ERS220Info::IRDistOffset]=GETSENSOR(ERS220Info::CPCSensorPSD) / 1000.0f; // Get acceleration sensors sensors[BAccelOffset] = GETD(ERS220Info::CPCSensorAccelFB); sensors[LAccelOffset] = GETD(ERS220Info::CPCSensorAccelLR); sensors[DAccelOffset] = GETD(ERS220Info::CPCSensorAccelUD); sensors[ERS220Info::ThermoOffset] = GETD(ERS220Info::CPCSensorThermoSensor); } // (ERS-7 only) if(robotDesign&ERS7Mask) { outputs[LFrLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointLFRotator); outputs[LFrLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointLFElevator); outputs[LFrLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointLFKnee); pidduties[LFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLFRotator); pidduties[LFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLFElevator); pidduties[LFrLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointLFKnee); outputs[RFrLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointRFRotator); outputs[RFrLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointRFElevator); outputs[RFrLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointRFKnee); pidduties[RFrLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRFRotator); pidduties[RFrLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRFElevator); pidduties[RFrLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointRFKnee); outputs[LBkLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointLHRotator); outputs[LBkLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointLHElevator); outputs[LBkLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointLHKnee); pidduties[LBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointLHRotator); pidduties[LBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointLHElevator); pidduties[LBkLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointLHKnee); outputs[RBkLegOffset + RotatorOffset ] = GETD(ERS7Info::CPCJointRHRotator); outputs[RBkLegOffset + ElevatorOffset ] = GETD(ERS7Info::CPCJointRHElevator); outputs[RBkLegOffset + KneeOffset ] = GETD(ERS7Info::CPCJointRHKnee); pidduties[RBkLegOffset + RotatorOffset ] = GETDUTY(ERS7Info::CPCJointRHRotator); pidduties[RBkLegOffset + ElevatorOffset] = GETDUTY(ERS7Info::CPCJointRHElevator); pidduties[RBkLegOffset + KneeOffset ] = GETDUTY(ERS7Info::CPCJointRHKnee); // Get head tilt,pan,nod joint angles outputs[HeadOffset+TiltOffset] = GETD(ERS7Info::CPCJointNeckTilt); outputs[HeadOffset+PanOffset ] = GETD(ERS7Info::CPCJointNeckPan); outputs[HeadOffset+RollOffset] = GETD(ERS7Info::CPCJointNeckNod); pidduties[HeadOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointNeckTilt); pidduties[HeadOffset+PanOffset ] = GETDUTY(ERS7Info::CPCJointNeckPan); pidduties[HeadOffset+RollOffset] = GETDUTY(ERS7Info::CPCJointNeckNod); outputs[ERS7Info::TailOffset+TiltOffset] = GETD(ERS7Info::CPCJointTailTilt); outputs[ERS7Info::TailOffset+PanOffset] = GETD(ERS7Info::CPCJointTailPan); pidduties[ERS7Info::TailOffset+TiltOffset] = GETDUTY(ERS7Info::CPCJointTailTilt); pidduties[ERS7Info::TailOffset+PanOffset] = GETDUTY(ERS7Info::CPCJointTailPan); outputs[ERS7Info::MouthOffset] = GETD(ERS7Info::CPCJointMouth); pidduties[ERS7Info::MouthOffset] = GETDUTY(ERS7Info::CPCJointMouth); if(lastState!=NULL && lastState!=this) { // Copy over new buttons buttons[ERS7Info::LFrPawOffset]=GETB(ERS7Info::CPCSwitchLFPaw); buttons[ERS7Info::RFrPawOffset]=GETB(ERS7Info::CPCSwitchRFPaw); buttons[ERS7Info::LBkPawOffset]=GETB(ERS7Info::CPCSwitchLHPaw); buttons[ERS7Info::RBkPawOffset]=GETB(ERS7Info::CPCSwitchRHPaw); // the sensors are scaled to be relatively similar to the pressure values given by the head on the 210 buttons[ ERS7Info::ChinButOffset]=GETSENSOR(ERS7Info::CPCSwitchChin); buttons[ ERS7Info::HeadButOffset]=GETSENSOR(ERS7Info::CPCSensorHead)/120; buttons[ ERS7Info::FrontBackButOffset]=GETSENSOR(ERS7Info::CPCSensorBackFront)/150; buttons[ ERS7Info::MiddleBackButOffset]=GETSENSOR(ERS7Info::CPCSensorBackMiddle)/150; buttons[ ERS7Info::RearBackButOffset]=GETSENSOR(ERS7Info::CPCSensorBackRear)/150; buttons[ ERS7Info::WirelessSwOffset]=GETSENSOR(ERS7Info::CPCSwitchWireless); //! process changes chkEvent(evtBuf,lastState); } else { // Get foot switches chkEvent(evtBuf,ERS7Info::LFrPawOffset,GETB(ERS7Info::CPCSwitchLFPaw),buttonNames[ERS7Info::LFrPawOffset]); chkEvent(evtBuf,ERS7Info::RFrPawOffset,GETB(ERS7Info::CPCSwitchRFPaw),buttonNames[ERS7Info::RFrPawOffset]); chkEvent(evtBuf,ERS7Info::LBkPawOffset,GETB(ERS7Info::CPCSwitchLHPaw),buttonNames[ERS7Info::LBkPawOffset]); chkEvent(evtBuf,ERS7Info::RBkPawOffset,GETB(ERS7Info::CPCSwitchRHPaw),buttonNames[ERS7Info::RBkPawOffset]); // Get buttons/switches // the sensors are scaled to be relatively similar to the pressure values given by the head on the 210 chkEvent(evtBuf, ERS7Info::ChinButOffset, GETSENSOR(ERS7Info::CPCSwitchChin), buttonNames[ERS7Info::ChinButOffset]); chkEvent(evtBuf, ERS7Info::HeadButOffset, GETSENSOR(ERS7Info::CPCSensorHead)/120, buttonNames[ERS7Info::HeadButOffset]); chkEvent(evtBuf, ERS7Info::FrontBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackFront)/150, buttonNames[ERS7Info::FrontBackButOffset]); chkEvent(evtBuf, ERS7Info::MiddleBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackMiddle)/150,buttonNames[ERS7Info::MiddleBackButOffset]); chkEvent(evtBuf, ERS7Info::RearBackButOffset, GETSENSOR(ERS7Info::CPCSensorBackRear)/150, buttonNames[ERS7Info::RearBackButOffset]); chkEvent(evtBuf, ERS7Info::WirelessSwOffset,GETSENSOR(ERS7Info::CPCSwitchWireless), buttonNames[ERS7Info::WirelessSwOffset]); } // Get IR distance sensor sensors[ERS7Info::NearIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorNearPSD) / 1000.0f; sensors[ERS7Info::FarIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorFarPSD) / 1000.0f; sensors[ERS7Info::ChestIRDistOffset] = GETSENSOR(ERS7Info::CPCSensorChestPSD) / 1000.0f; // Get acceleration sensors sensors[BAccelOffset] = GETD(ERS7Info::CPCSensorAccelFB); sensors[LAccelOffset] = GETD(ERS7Info::CPCSensorAccelLR); sensors[DAccelOffset] = GETD(ERS7Info::CPCSensorAccelUD); } //unsigned int dif=curtime-(lastState==NULL ? lastSensorUpdateTime : lastState->lastSensorUpdateTime); lastSensorUpdateTime=curtime; frameNumber=newFrameNumber; framesProcessed=(lastState!=NULL?lastState->framesProcessed:framesProcessed)+1; for(unsigned int i=0; i<evtBuf.size(); i++) er->postEvent(evtBuf[i]); //Apply sensor calibrations (currently only joint positions - perhaps IR as well?) for(unsigned int i=0; i<NumPIDJoints; i++) outputs[PIDJointOffset+i]*=config->motion.calibration[i]; //this version of read doesn't post the sensor update event -- the caller should do that //this event gets posted by MMCombo only if there's no back log on the interprocess event queue (don't want to stack these up for sensor frames missed by main) //er->postEvent(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID,dif,"SensorSouceID::UpdatedSID",1); } /*! This will cause events to be posted */ void WorldState::read(const OPowerStatus& power, EventRouter* er) { std::string actnames[PowerSourceID::NumPowerSIDs]; std::string denames[PowerSourceID::NumPowerSIDs]; unsigned int actmasks[PowerSourceID::NumPowerSIDs]; memset(actmasks,0,sizeof(unsigned int)*PowerSourceID::NumPowerSIDs); //RobotStatus chkPowerEvent(PowerSourceID::PauseSID, power.robotStatus,orsbPAUSE, "Pause",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::MotorPowerSID, power.robotStatus,orsbMOTOR_POWER, "MotorPower",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::VibrationSID, power.robotStatus,orsbVIBRATION_DETECT, "Vibration",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ExternalPortSID, power.robotStatus,orsbEX_PORT_CONNECTED, "ExternalPort",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::StationConnectSID, power.robotStatus,orsbSTATION_CONNECTED, "StationConnect",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ExternalPowerSID, power.robotStatus,orsbEX_POWER_CONNECTED, "ExternalPower",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::BatteryConnectSID, power.robotStatus,orsbBATTERY_CONNECTED, "BatteryConnect",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ChargingSID, power.robotStatus,orsbBATTERY_CHARGING, "BatteryCharging",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::BatteryFullSID, power.robotStatus,orsbBATTERY_CAPACITY_FULL, "BatteryFull",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.robotStatus,orsbBATTERY_CAPACITY_LOW, "BatteryLow",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::OverChargedSID, power.robotStatus,orsbBATTERY_OVER_CURRENT, "BatteryOverCurrent",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::OverheatingSID, power.robotStatus,orsbBATTERY_OVER_TEMP_DISCHARGING,"BatteryOverTempDischarge",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::OverheatingSID, power.robotStatus,orsbBATTERY_OVER_TEMP_CHARGING, "BatteryOverTempCharge",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbBATTERY_ERROR_OF_CHARGING, "BatteryChargeError",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbERROR_OF_PLUNGER, "PlungerError",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::PowerGoodSID, power.robotStatus,orsbOPEN_R_POWER_GOOD, "PowerGood",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbERROR_OF_FAN, "FanError",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::DataFromStationSID,power.robotStatus,orsbDATA_STREAM_FROM_STATION, "DataFromStation",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::RegisterUpdateSID, power.robotStatus,orsbREGISTER_UPDATED_BY_STATION, "RegisterUpdate",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ErrorSID, power.robotStatus,orsbRTC_ERROR, "RTCError",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::RTCSID, power.robotStatus,orsbRTC_OVERFLOW, "RTCOverflow",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::RTCSID, power.robotStatus,orsbRTC_RESET, "RTCReset",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::RTCSID, power.robotStatus,orsbRTC_SET, "RTCSet",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::SpecialModeSID, power.robotStatus,orsbSPECIAL_MODE, "SpecialMode",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::BMNDebugModeSID, power.robotStatus,orsbBMN_DEBUG_MODE, "BMNDebugMode",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::ChargerStatusSID, power.robotStatus,orsbCHARGER_STATUS, "ChargerStatus",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::PlungerSID, power.robotStatus,orsbPLUNGER, "Plunger",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::SuspendedSID, power.robotStatus,orsbSUSPENDED, "Suspended",actnames,denames,actmasks); //BatteryStatus chkPowerEvent(PowerSourceID::ErrorSID, power.batteryStatus,obsbERROR_CODE_MASK, "BatteryError",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::BatteryEmptySID, power.batteryStatus,obsbFULLY_DISCHARGED, "FullyDischarged",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::BatteryFullSID, power.batteryStatus,obsbFULLY_CHARGED, "FullyCharged",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::DischargingSID, power.batteryStatus,obsbDISCHARGING, "Discharging",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::BatteryInitSID, power.batteryStatus,obsbINITIALIZED, "BatteryInit",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_TIME_ALARM, "RemainingTimeAlarm",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::LowPowerWarnSID, power.batteryStatus,obsbREMAINING_CAPACITY_ALARM, "RemainingCapacityAlarm",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::TermDischargeSID,power.batteryStatus,obsbTERMINATED_DISCHARGING_ALARM,"TermDischargeAlarm",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::OverheatingSID, power.batteryStatus,obsbOVER_TEMP_ALARM, "OverTempAlarm",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::TermChargeSID, power.batteryStatus,obsbTERMINATED_CHARGING_ALARM, "TermChargeAlarm",actnames,denames,actmasks); chkPowerEvent(PowerSourceID::OverChargedSID, power.batteryStatus,obsbOVER_CHARGED_ALARM, "OverChargeAlarm",actnames,denames,actmasks); sensors[PowerRemainOffset] = power.remainingCapacity/100.0; sensors[PowerThermoOffset] = power.temperature/100.0; sensors[PowerCapacityOffset] = power.fullyChargedCapacity; sensors[PowerVoltageOffset] = power.voltage/1000.0; sensors[PowerCurrentOffset] = power.current; //only generate status events when a change happens for(unsigned int i=0; i<PowerSourceID::NumPowerSIDs; i++) { if(actmasks[i]) { //now on if(!powerFlags[i]) //was off: activation er->postEvent(EventBase::powerEGID,i,EventBase::activateETID,0,actnames[i],1); else if(actmasks[i]!=powerFlags[i]) //already on - change? : status er->postEvent(EventBase::powerEGID,i,EventBase::statusETID,0,actnames[i],1); } else { // now off if(powerFlags[i]) //was on: deactivation er->postEvent(EventBase::powerEGID,i,EventBase::deactivateETID,0,denames[i],0); } powerFlags[i]=actmasks[i]; } er->postEvent(EventBase::powerEGID,PowerSourceID::UpdatedSID,EventBase::statusETID,0,"PowerSourceID::UpdatedSID",1); } #else //PLATFORM_LOCAL /*! This version of read() doesn't post the sensor update event -- the caller should do that * (this variant is used by the simulator, different parts fill in different sections, the last one should trigger the event) * * This function assumes that the pose (and any associated sensor readings) * are for the current model robot, and that any calibration has already * been applied (so the values can be used unchanged) */ void WorldState::read(const PostureEngine& pose, WorldState* lastState, EventRouter* er) { curtime=get_time(); std::vector<EventBase> evtBuf; WorldState* ext=pose.getLoadedSensors(); unsigned int newFrameNumber=(lastState!=NULL?lastState->frameNumber:frameNumber)+NumFrames; for(unsigned int i=0; i<NumOutputs; i++) if(pose(i).weight>0) outputs[i]=pose(i).value; if(frameNumber>=newFrameNumber) return; //sensors have already been filled in if(lastState!=NULL && lastState!=this) { alwaysGenerateStatus=lastState->alwaysGenerateStatus; //pid and velocity values will be copied at "completion" robotStatus=lastState->robotStatus; batteryStatus=lastState->batteryStatus; for(unsigned int i=0; i<PowerSourceID::NumPowerSIDs; i++) powerFlags[i]=lastState->powerFlags[i]; //important part -- copy over button_times before calls to chkEvent for(unsigned int i=0; i<NumButtons; i++) button_times[i]=lastState->button_times[i]; } if(ext!=NULL) { if(ext==this) { chkEvent(evtBuf,lastState); } else { if(lastState!=NULL && lastState!=this) { for(unsigned int i=0; i<NumButtons; i++) buttons[i]=ext->buttons[i]; chkEvent(evtBuf,lastState); } else { for(unsigned int i=0; i<NumButtons; i++) chkEvent(evtBuf,i,ext->buttons[i],buttonNames[i]); } for(unsigned int i=0; i<NumSensors; i++) sensors[i]=ext->sensors[i]; for(unsigned int i=0; i<NumPIDJoints; i++) pidduties[i]=ext->pidduties[i]; } } else if(lastState!=NULL && lastState!=this) { // no loaded sensors at all -- copy over previous values for(unsigned int i=0; i<NumButtons; i++) buttons[i]=lastState->buttons[i]; for(unsigned int i=0; i<NumSensors; i++) sensors[i]=lastState->sensors[i]; for(unsigned int i=0; i<NumPIDJoints; i++) pidduties[i]=lastState->pidduties[i]; } //unsigned int dif=curtime-(lastState==NULL ? lastSensorUpdateTime : lastState->lastSensorUpdateTime); lastSensorUpdateTime=curtime; if(newFrameNumber!=-1U) frameNumber=newFrameNumber; framesProcessed=(lastState!=NULL?lastState->framesProcessed:framesProcessed)+1; for(unsigned int i=0; i<evtBuf.size(); i++) er->postEvent(evtBuf[i]); //this version of read doesn't post the sensor update event -- the caller should do that // (this variant is used by the simulator, different parts fill in different sections, the last one should trigger the event) //er->postEvent(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID,dif,"SensorSourceID::UpdatedSID",1); } #endif //platform-specific sensor updating void WorldState::chkEvent(std::vector<EventBase>& evtBuf, unsigned int sid, float newval, const char* name) { if(newval>=0.1) { //now on if(buttons[sid]<0.1) { //was off: activation cout << ProcessID::getIDStr() << " post activate" << endl; evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::activateETID,0,name,newval)); button_times[sid]=curtime; } else if(alwaysGenerateStatus || buttons[sid]!=newval) { //already on - always or change? : status unsigned int dur=curtime-button_times[sid]; cout << ProcessID::getIDStr() << " post status" << endl; evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::statusETID,dur,name,newval)); } } else { //now off if(buttons[sid]>=0.1) { //was on: deactivation unsigned int dur=curtime-button_times[sid]; button_times[sid]=0; cout << ProcessID::getIDStr() << " post deactivate" << endl; evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::deactivateETID,dur,name,0)); } } //update value buttons[sid]=newval; } void WorldState::chkEvent(std::vector<EventBase>& evtBuf, WorldState* lastState) { // posture already loaded data into this instance of WorldState // variation on chkEvent -- instead, we already hold the latest data, need to compare against lastState for(unsigned int sid=0; sid<NumButtons; sid++) { if(buttons[sid]>=0.1) { //now on if(lastState->buttons[sid]<0.1) { //was off: activation evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::activateETID,0,buttonNames[sid],buttons[sid])); button_times[sid]=curtime; } else if(alwaysGenerateStatus || lastState->buttons[sid]!=buttons[sid]) { //already on - always or change? : status unsigned int dur=curtime-button_times[sid]; evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::statusETID,dur,buttonNames[sid],buttons[sid])); } } else { //now off if(lastState->buttons[sid]>=0.1) { //was on: deactivation unsigned int dur=curtime-button_times[sid]; button_times[sid]=0; evtBuf.push_back(EventBase(EventBase::buttonEGID,sid,EventBase::deactivateETID,dur,buttonNames[sid],0)); } } } } /*! @file * @brief Implements WorldState, maintains information about the robot's environment, namely sensors and power status * @author ejt (Creator) * * $Author: ejt $ * $Name: tekkotsu-3_0 $ * $Revision: 1.40 $ * $State: Exp $ * $Date: 2006/09/29 16:56:09 $ */

Tekkotsu v3.0

Generated Wed Oct 4 00:03:47 2006 by Doxygen 1.4.7