Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

TorqueCalibrate.cc

Go to the documentation of this file.
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 //better to put this here instead of the header
00014 using namespace std; 
00015 
00016 
00017 //***************************//
00018 //***** TorqueCalibrate *****//
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 //***** TorqueCalibrate::TakeMeasurementControl *****//
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         // move slowly at first in case not quite against sensor
00144         ms->setOutputCmd(joint,basePosition+offset/4);
00145         ms->advanceTime(300);
00146         ms->setOutputCmd(joint,basePosition+offset/2);
00147         ms->advanceTime(200);
00148         // hard push at the end
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 /*! @file
00207  * @brief 
00208  * @author Ethan Tira-Thompson (ejt) (Creator)
00209  *
00210  * $Author: ejt $
00211  * $Name: tekkotsu-4_0 $
00212  * $Revision: 1.4 $
00213  * $State: Exp $
00214  * $Date: 2007/11/13 04:16:00 $
00215  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:56 2007 by Doxygen 1.5.4