00001 #include "TorqueCalibrate.h"
00002 #include "Behaviors/Controls/NullControl.h"
00003 #include "Shared/WorldState.h"
00004 #include "Shared/Config.h"
00005 #include "Events/EventRouter.h"
00006 #include "Motion/MMAccessor.h"
00007 #include "Motion/PIDMC.h"
00008 #include "Motion/MotionSequenceMC.h"
00009 #include "Sound/SoundManager.h"
00010 #include <fstream>
00011 #include <cmath>
00012
00013
00014 using namespace std;
00015
00016
00017
00018
00019
00020
00021 void TorqueCalibrate::record(unsigned int joint, float sensorDist, float maxDuty, float maxForce) const {
00022 std::ofstream log(filename.c_str(),std::ios::app);
00023 if(!log) {
00024 serr->printf("ERROR: could not open %s for writing log\n",filename.c_str());
00025 sndman->playFile("fart.wav");
00026 } else {
00027 log << joint << '\t' << sensorDist << '\t' << maxDuty << '\t' << maxForce << std::endl;
00028 sndman->playFile("camera.wav");
00029 }
00030 std::cout << "DATA: " << joint << '\t' << sensorDist << '\t' << maxDuty << '\t' << maxForce << std::endl;
00031 }
00032
00033 void TorqueCalibrate::refresh() {
00034 if(filenameInput->getLastInput()!=filename) {
00035 filename=filenameInput->getLastInput();
00036 std::string::size_type f=filename.rfind("/");
00037 filenameInput->setName("Storage: "+filename.substr(f==string::npos?0:f+1));
00038 std::string desc="Location where data will be appended to any previous contents";
00039 filenameInput->setDescription(desc+": "+filename);
00040 std::ofstream log(filename.c_str(),std::ios::app);
00041 if(!log)
00042 serr->printf("ERROR: could not open %s for writing log\n",filename.c_str());
00043 }
00044 ControlBase::refresh();
00045 }
00046
00047
00048
00049
00050
00051
00052 ControlBase * TorqueCalibrate::TakeMeasurementControl::activate(MotionManager::MC_ID disp_id, Socket * gui) {
00053 #ifdef TGT_HAS_BUTTONS
00054 # ifdef TGT_ERS7
00055 erouter->addListener(this,EventBase::buttonEGID,FrontBackButOffset,EventBase::activateETID);
00056 erouter->addListener(this,EventBase::buttonEGID,MiddleBackButOffset,EventBase::activateETID);
00057 erouter->addListener(this,EventBase::buttonEGID,RearBackButOffset,EventBase::activateETID);
00058 # else
00059 erouter->addListener(this,EventBase::buttonEGID,BackButOffset,EventBase::activateETID);
00060 # endif
00061 #else
00062 # warning TorqueCalibrate control needs some kind of trigger, target model does not have any buttons?
00063 #endif
00064 cstate=ZERO_JOINT;
00065 SharedObject<PIDMC> pidmc(0);
00066 pidID=motman->addPersistentMotion(pidmc,MotionManager::kHighPriority);
00067 return ControlBase::activate(disp_id,gui);
00068 }
00069
00070 void TorqueCalibrate::TakeMeasurementControl::processEvent(const EventBase& event) {
00071 if(cstate==ZERO_JOINT && event.getGeneratorID()==EventBase::buttonEGID) {
00072 basePosition=state->outputs[joint];
00073 transition(RECORD_POSITION);
00074 } else if(cstate==DO_PULSE && event.getGeneratorID()==EventBase::sensorEGID) {
00075 if(std::abs(state->pidduties[joint-PIDJointOffset]) > maxDuty)
00076 maxDuty=std::abs(state->pidduties[joint-PIDJointOffset]);
00077 std::cout << "Duty: " << state->pidduties[joint-PIDJointOffset] << std::endl;
00078 } else if(cstate==DO_PULSE && event.getGeneratorID()==EventBase::timerEGID) {
00079 erouter->removeListener(this,EventBase::sensorEGID);
00080 } else if(cstate==DO_PULSE && event.getGeneratorID()==EventBase::motmanEGID) {
00081 pulseID=MotionManager::invalid_MC_ID;
00082 std::cout << "Max duty: " << maxDuty << std::endl;
00083 transition(RECORD_FORCE);
00084 } else {
00085 std::cerr << "Unhandled event " << event.getName() << std::endl;
00086 }
00087 }
00088
00089 void TorqueCalibrate::TakeMeasurementControl::refresh() {
00090 clearSlots();
00091 switch(cstate) {
00092 case ZERO_JOINT:
00093 pushSlot(new NullControl("Position the joint"));
00094 pushSlot(new NullControl("and press a back button"));
00095 break;
00096 case RECORD_POSITION:
00097 pushSlot(new NullControl("What is the length of"));
00098 pushSlot(new NullControl("the lever arm? (cm)"));
00099 pushSlot(new NullControl("(Dist. from axis of"));
00100 pushSlot(new NullControl("rot. to force sensor)"));
00101 break;
00102 case INPUT_PULSE:
00103 pushSlot(new NullControl("Enter the position"));
00104 pushSlot(new NullControl("offset to attempt"));
00105 pushSlot(new NullControl("(radians -- bigger"));
00106 pushSlot(new NullControl("offset means apply"));
00107 pushSlot(new NullControl("more force...)"));
00108 break;
00109 case DO_PULSE:
00110 pushSlot(new NullControl("Running..."));
00111 break;
00112 case RECORD_FORCE: {
00113 char res[256];
00114 snprintf(res,256,"Max duty was: %g",maxDuty);
00115 pushSlot(new NullControl(res));
00116 pushSlot(NULL);
00117 pushSlot(new NullControl("Enter the maximum"));
00118 pushSlot(new NullControl("force (N)"));
00119 break;
00120 }
00121 }
00122 ControlBase::refresh();
00123 }
00124
00125 ControlBase * TorqueCalibrate::TakeMeasurementControl::takeInput(const std::string& msg) {
00126 switch(cstate) {
00127 case ZERO_JOINT:
00128 case DO_PULSE:
00129 break;
00130 case RECORD_POSITION:
00131 sensorDist=atof(msg.c_str());
00132 if(sensorDist==0) {
00133 sndman->playFile("fart.wav");
00134 return NULL;
00135 }
00136 transition(INPUT_PULSE);
00137 break;
00138 case INPUT_PULSE: {
00139 float offset=atof(msg.c_str());
00140 SharedObject<TinyMotionSequenceMC> ms;
00141 ms->advanceTime(350);
00142 if(offset>.1) {
00143
00144 ms->setOutputCmd(joint,basePosition+offset/4);
00145 ms->advanceTime(300);
00146 ms->setOutputCmd(joint,basePosition+offset/2);
00147 ms->advanceTime(200);
00148
00149 ms->setOutputCmd(joint,basePosition+offset);
00150 ms->advanceTime(500);
00151 } else {
00152 ms->setOutputCmd(joint,basePosition+offset);
00153 ms->advanceTime(700);
00154 }
00155 ms->setOutputCmd(joint,basePosition+offset);
00156 ms->advanceTime(300);
00157 erouter->addTimer(this,0,ms->getTime(),false);
00158 ms->setOutputCmd(joint,basePosition);
00159 ms->advanceTime(200);
00160 ms->setOutputCmd(joint,basePosition);
00161 pulseID=motman->addPrunableMotion(ms);
00162 transition(DO_PULSE);
00163 } break;
00164 case RECORD_FORCE: {
00165 float f=atof(msg.c_str());
00166 if(f!=0)
00167 parent.record(joint,sensorDist,maxDuty,f);
00168 else
00169 sndman->playFile("fart.wav");
00170 transition(INPUT_PULSE);
00171 } break;
00172 }
00173 return this;
00174 }
00175
00176 void TorqueCalibrate::TakeMeasurementControl::deactivate() {
00177 erouter->remove(this);
00178 motman->removeMotion(pidID);
00179 pidID=MotionManager::invalid_MC_ID;
00180 if(pulseID!=MotionManager::invalid_MC_ID) {
00181 motman->removeMotion(pulseID);
00182 pulseID=MotionManager::invalid_MC_ID;
00183 }
00184 ControlBase::deactivate();
00185 }
00186
00187 void TorqueCalibrate::TakeMeasurementControl::transition(State_t newstate) {
00188 MMAccessor<PIDMC>(pidID)->setAllPowerLevel(1);
00189 erouter->removeListener(this);
00190 cstate=newstate;
00191 if(cstate==RECORD_POSITION || cstate==INPUT_PULSE || cstate==RECORD_FORCE)
00192 sndman->playFile("ping.wav");
00193 else
00194 sndman->playFile("barkhigh.wav");
00195 refresh();
00196 if(cstate==DO_PULSE) {
00197 maxDuty=0;
00198 float pidSetting[] = {DefaultPIDs[joint][0],0,0};
00199 MMAccessor<PIDMC>(pidID)->setPID(joint,pidSetting);
00200 erouter->addListener(this,EventBase::sensorEGID);
00201 erouter->addListener(this,EventBase::motmanEGID,pulseID,EventBase::deactivateETID);
00202 }
00203 }
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215