00001
00002
00004
00005 #include "BHandDriver.h"
00006
00007 #include "BHandSupervisoryRealTime.h"
00008
00009
00010 int BHandDriver::runSupervisoryCommand(BHandSupervisoryCommand *command)
00011 {
00012
00013
00014 int result;
00015
00016 if (m_moduleSuperReal->getSyncMode() == BHMODE_ASYNCNOW)
00017 {
00018
00019 if ((result = m_moduleSuperReal->ComWaitForCompletion(0)))
00020 return BHERR_NOTCOMPLETED;
00021 }
00022 else
00023 {
00024
00025 m_moduleSuperReal->ComWaitForCompletion(INFINITE);
00026 }
00027
00028 setActiveSupervisoryCommand(command);
00029
00030
00031 bool immediate = false;
00032 if (command != NULL && (
00033 command->getCommand() == BHSupervisoryReset ||
00034 command->getCommand() == BHSupervisoryGet ||
00035 command->getCommand() == BHSupervisoryPGet ||
00036 command->getCommand() == BHSupervisoryTemperature ||
00037 command->getCommand() == BHSupervisoryCmd))
00038 immediate = (m_moduleSuperReal->getSyncMode() == BHMODE_RETURN);
00039 if (immediate)
00040 m_moduleSuperReal->setSyncMode(BHMODE_SYNC);
00041 result = m_moduleSuperReal->ComRequest(BHREQ_SUPERVISE);
00042 if (immediate)
00043 m_moduleSuperReal->setSyncMode(BHMODE_RETURN);
00044
00045 return result;
00046 }
00047
00049
00051
00052 int BHandDriver::InitHand(const char *motor)
00053 {
00054 return runSupervisoryCommand(new BHandCommandInit(toBHMotors(motor)));
00055 }
00056
00057 int BHandDriver::Reset(bool *responseReceived)
00058 {
00059
00060
00061
00062 if (responseReceived)
00063 *responseReceived = false;
00064
00065
00066
00067 int r;
00068 if ((r = runSupervisoryCommand(new BHandCommandReset())))
00069 {
00070
00071 return r;
00072 }
00073
00074
00075
00076 if (!m_moduleSuperReal->ComIsPending() && responseReceived)
00077
00078
00079 *responseReceived = ((BHandResultReset *)m_supervisoryCommandResult)->responseReceived();
00080
00081 return 0;
00082 }
00083
00084 int BHandDriver::Close(const char *motor)
00085 {
00086 return runSupervisoryCommand(new BHandCommandClose(toBHMotors(motor)));
00087 }
00088
00089 int BHandDriver::Open(const char *motor)
00090 {
00091 return runSupervisoryCommand(new BHandCommandOpen(toBHMotors(motor)));
00092 }
00093
00094 int BHandDriver::GoToDefault(const char *motor)
00095 {
00096 return runSupervisoryCommand(new BHandCommandGoToDefault(toBHMotors(motor)));
00097 }
00098
00099 int BHandDriver::GoToDifferentPositions(const int *encoderPositions, unsigned int numEncoderPositions)
00100 {
00101 return runSupervisoryCommand(new BHandCommandGoToDifferentPositions(encoderPositions, numEncoderPositions));
00102 }
00103
00104 int BHandDriver::GoToHome(const char *motor)
00105 {
00106 return runSupervisoryCommand(new BHandCommandGoToHome(toBHMotors(motor)));
00107 }
00108
00109 int BHandDriver::GoToPosition(const char *motor, int encoderPositionTickCount)
00110 {
00111 return runSupervisoryCommand(new BHandCommandGoToPosition(toBHMotors(motor), encoderPositionTickCount));
00112 }
00113
00114 int BHandDriver::StepClose(const char *motor, bool valueIncluded, int stepAmount)
00115 {
00116 return runSupervisoryCommand(new BHandCommandStepClose(toBHMotors(motor), valueIncluded, stepAmount));
00117 }
00118
00119 int BHandDriver::StepOpen(const char *motor, bool valueIncluded, int stepAmount)
00120 {
00121 return runSupervisoryCommand(new BHandCommandStepOpen(toBHMotors(motor), valueIncluded, stepAmount));
00122 }
00123
00124 int BHandDriver::StopMotor(const char *motor)
00125 {
00126 return runSupervisoryCommand(new BHandCommandStopMotor(toBHMotors(motor)));
00127 }
00128
00129 int BHandDriver::TorqueClose(const char *motor)
00130 {
00131 return runSupervisoryCommand(new BHandCommandTorqueClose(toBHMotors(motor)));
00132 }
00133
00134 int BHandDriver::TorqueOpen(const char *motor)
00135 {
00136 return runSupervisoryCommand(new BHandCommandTorqueOpen(toBHMotors(motor)));
00137 }
00138
00139
00140 int BHandDriver::Get(const char *motor, const char *parameter, int *result)
00141 {
00142
00143
00144
00145 BHMotors bhMotors = toBHMotors(motor);
00146 for (unsigned int i = 0; i < countMotors(bhMotors); i++)
00147 result[i] = 20 + i * 2;
00148
00149 int r;
00150 if ((r = runSupervisoryCommand(new BHandCommandGet(bhMotors, parameter))))
00151 return r;
00152
00153 if (!m_moduleSuperReal->ComIsPending())
00154 {
00155
00156 BHandResultGet *getResult = (BHandResultGet *)m_supervisoryCommandResult;
00157
00158
00159 for (unsigned int i = 0; i < getResult->getNumResults(); i++)
00160 result[i] = getResult->getResult(i);
00161 }
00162
00163 return 0;
00164 }
00165
00166 int BHandDriver::Set(const char *motor, const char *parameter, int value)
00167 {
00168 return runSupervisoryCommand(new BHandCommandSet(toBHMotors(motor), parameter, value));
00169 }
00170
00171 int BHandDriver::PGet(const char *parameter, int *result)
00172 {
00173
00174
00175
00176 *result = 0;
00177
00178
00179 int r;
00180 if ((r = runSupervisoryCommand(new BHandCommandPGet(parameter))))
00181 return r;
00182
00183 if (!m_moduleSuperReal->ComIsPending())
00184 {
00185
00186
00187 *result = ((BHandResultPGet *)m_supervisoryCommandResult)->getResult();
00188 }
00189
00190 return 0;
00191 }
00192
00193 int BHandDriver::PSet(const char *parameter, int value)
00194 {
00195 return runSupervisoryCommand(new BHandCommandPSet(parameter, value));
00196 }
00197
00198 int BHandDriver::Default(const char *motor)
00199 {
00200 return runSupervisoryCommand(new BHandCommandDefault(toBHMotors(motor)));
00201 }
00202
00203 int BHandDriver::Load(const char *motor)
00204 {
00205 return runSupervisoryCommand(new BHandCommandLoad(toBHMotors(motor)));
00206 }
00207
00208 int BHandDriver::Save(const char *motor)
00209 {
00210 return runSupervisoryCommand(new BHandCommandSave(toBHMotors(motor)));
00211 }
00212
00213 int BHandDriver::Temperature(int *temperature)
00214 {
00215
00216
00217
00218 *temperature = 0;
00219
00220 int r;
00221 if ((r = runSupervisoryCommand(new BHandCommandTemperature())))
00222 return r;
00223
00224 if (!m_moduleSuperReal->ComIsPending())
00225
00226
00227 *temperature = ((BHandResultTemperature *)m_supervisoryCommandResult)->getTemperature();
00228
00229 return 0;
00230
00231 }
00232
00233 int BHandDriver::Delay(unsigned int msec)
00234 {
00235 return runSupervisoryCommand(new BHandCommandDelay(msec));
00236 }
00237
00238 int BHandDriver::Command(const char *send)
00239 {
00240 return runSupervisoryCommand(new BHandCommandCommand(send));
00241 }
00242
00243 int BHandDriver::Command(const char *send, char *receive)
00244 {
00245
00246
00247
00248
00249 if (receive)
00250 *receive = 0;
00251
00252 int r;
00253 if ((r = runSupervisoryCommand(new BHandCommandCommand(send))))
00254 return r;
00255
00256 if (receive && !m_moduleSuperReal->ComIsPending())
00257
00258
00259 {
00260 if (m_supervisoryCommandResult != NULL)
00261 strcpy(receive, ((BHandResultCommand *)m_supervisoryCommandResult)->getReceive());
00262 else
00263 *receive = 0;
00264 }
00265
00266 return 0;
00267 }
00268
00269 int BHandDriver::Baud(unsigned int newbaud)
00270 {
00271 return runSupervisoryCommand(new BHandCommandBaud(newbaud));
00272 }
00273
00274
00275 int BHandDriver::ExecuteSupervisoryCall()
00276 {
00277 BHandSupervisoryCommand *command = m_activeSupervisoryCommand;
00278 if (command == NULL)
00279 return HandleSupervisoryCall();
00280
00281
00282 if (m_supervisoryCommandResult != NULL)
00283 {
00284
00285 delete m_supervisoryCommandResult;
00286 m_supervisoryCommandResult = NULL;
00287 }
00288
00289 int r = -1;
00290
00291 switch (command->getCommand())
00292 {
00293 case BHSupervisoryInit: { r = HandInit(((BHandCommandInit *)command)->Motors); break; }
00294 case BHSupervisoryReset: {
00295 bool reset;
00296 r = HandReset(((BHandCommandReset *)command)->Motors, &reset);
00297
00298 m_supervisoryCommandResult = new BHandResultReset(reset);
00299 break;
00300 }
00301
00302 case BHSupervisoryClose: { r = HandClose(((BHandCommandClose *)command)->Motors); break; }
00303 case BHSupervisoryOpen: { r = HandOpen(((BHandCommandOpen *)command)->Motors); break; }
00304
00305 case BHSupervisoryTorqueClose: { r = HandTorqueClose(((BHandCommandTorqueClose *)command)->Motors); break; }
00306 case BHSupervisoryTorqueOpen: { r = HandTorqueOpen(((BHandCommandTorqueOpen *)command)->Motors); break; }
00307
00308 case BHSupervisoryStepClose: { r = HandStepClose(((BHandCommandStepClose *)command)->Motors, ((BHandCommandStepClose *)command)->ValueIncluded, ((BHandCommandStepClose *)command)->StepAmount); break; }
00309 case BHSupervisoryStepOpen: { r = HandStepOpen(((BHandCommandStepOpen *)command)->Motors, ((BHandCommandStepOpen *)command)->ValueIncluded, ((BHandCommandStepOpen *)command)->StepAmount); break; }
00310
00311 case BHSupervisoryGoToDefault: { r = HandGoToDefault(((BHandCommandGoToDefault *)command)->Motors, ((BHandCommandGoToDefault *)command)->ValueIncluded, ((BHandCommandGoToDefault *)command)->DefaultPosition); break; }
00312 case BHSupervisoryGoToDifferentPositions: { r = HandGoToDifferentPositionsHand(((BHandCommandGoToDifferentPositions *)command)->getEncoderPositions(), ((BHandCommandGoToDifferentPositions *)command)->getNumEncoderPositions()); break; }
00313 case BHSupervisoryGoToHome: { r = HandGoToHome(((BHandCommandGoToHome *)command)->Motors); break; }
00314 case BHSupervisoryGoToPosition: { r = HandGoToPosition(((BHandCommandGoToPosition *)command)->Motors, ((BHandCommandGoToPosition *)command)->EncoderPositionTickCount); break; }
00315
00316 case BHSupervisoryGet: {
00317 int nResults = 4;
00318 if (getComm() == BH_SERIAL_COMMUNICATION && countMotors(((BHandCommandGet *)command)->Motors) == 1)
00319
00320 nResults = countWords(((BHandCommandGet *)command)->getProperty());
00321
00322 int *results = new int[nResults];
00323
00324 int n = -1;
00325 r = HandGet(((BHandCommandGet *)command)->Motors, ((BHandCommandGet *)command)->getProperty(), results, &n);
00326
00327
00328
00329 if (n == -1)
00330 m_supervisoryCommandResult = new BHandResultGet(((BHandCommandGet *)command)->Motors, results);
00331 else
00332 m_supervisoryCommandResult = new BHandResultGet(((BHandCommandGet *)command)->Motors, results, n >= 0 ? n : nResults);
00333
00334 delete[] results;
00335 break;
00336 }
00337
00338
00339
00340
00341
00342
00343
00344
00345 case BHSupervisorySet: { r = HandSet(((BHandCommandSet *)command)->Motors, ((BHandCommandSet *)command)->getProperty(), ((BHandCommandSet *)command)->Value); break; }
00346
00347 case BHSupervisoryPGet: {
00348 int result;
00349 r = HandPGet(((BHandCommandPGet *)command)->getProperty(), &result);
00350
00351 m_supervisoryCommandResult = new BHandResultPGet(result);
00352 break;
00353 }
00354 case BHSupervisoryPSet: { r = HandPSet(((BHandCommandPSet *)command)->getProperty(), ((BHandCommandPSet *)command)->Value); break; }
00355
00356 case BHSupervisorySave: { r = HandSave(((BHandCommandSave *)command)->Motors); break; }
00357 case BHSupervisoryLoad: { r = HandLoad(((BHandCommandLoad *)command)->Motors); break; }
00358 case BHSupervisoryDefault: { r = HandDefault(((BHandCommandDefault *)command)->Motors); break; }
00359
00360 case BHSupervisoryTemperature: {
00361 if (getComm() == BH_SERIAL_COMMUNICATION)
00362 {
00363 int temperature;
00364 r = HandTemperature(((BHandCommandTemperature *)command)->Motors, &temperature, 1);
00365
00366 m_supervisoryCommandResult = new BHandResultTemperature(temperature);
00367 }
00368 else
00369 {
00370 int temperatures[4];
00371 r = HandTemperature(((BHandCommandTemperature *)command)->Motors, temperatures, 4);
00372
00373 m_supervisoryCommandResult = new BHandResultTemperature(temperatures, 4);
00374 }
00375 break;
00376 }
00377
00378 case BHSupervisoryBaud: { r = HandBaud(((BHandCommandBaud *)command)->NewBaud); break; }
00379 case BHSupervisoryDelay: { r = HandDelay(((BHandCommandDelay *)command)->MilliSeconds); break; }
00380 case BHSupervisoryStopMotor: { r = HandStopMotor(((BHandCommandStopMotor *)command)->Motors); break; }
00381
00382 case BHSupervisoryCmd:
00383 {
00384 char *receive = HandCommand(((BHandCommandCommand *)command)->getSend(), &r);
00385
00386 if (!r)
00387 m_supervisoryCommandResult = new BHandResultCommand(receive);
00388
00389 break;
00390 }
00391 default:
00392 {
00393 return BHERR_NOTCOMPLETED;
00394 }
00395 }
00396
00397
00398 delete m_activeSupervisoryCommand;
00399 m_activeSupervisoryCommand = NULL;
00400
00401 return r;
00402 }