bool DynamixelServo::setWheelMode(bool enable, bool broadcast) { if(enable) ///setting wheel mode { this->setSpeed(0.1,broadcast); byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); if(broadcast) byteArray.append((char)BROADCAST); else byteArray.append((char)id); byteArray.append((char)0x07); byteArray.append((char)WRITE_DATA); byteArray.append((char)CW_ANGLE_LIMIT); byteArray.append((char)0x00); byteArray.append((char)0x00); byteArray.append((char)0x00); byteArray.append((char)0x00); addCRC(byteArray); send(byteArray); this->stop(true,broadcast); } else ///getting back from wheel mode { //this->stop(true,broadcast); this->setSpeed(0.1,broadcast); this->setTorqueOFF(broadcast); byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x07); byteArray.append((char)WRITE_DATA); byteArray.append((char)CW_ANGLE_LIMIT); byteArray.append((char)this->CWAngleLimitLB); byteArray.append((char)this->CWAngleLimitHB); byteArray.append((char)this->CCWAngleLimitLB); byteArray.append((char)this->CCWAngleLimitHB); addCRC(byteArray); send(byteArray); this->setTorqueOFF(broadcast); } return true; }
bool DynamixelServo::setAngle(double angle, bool now) { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x05); if(now == false) //writes data to register, waits for function updateServo byteArray.append((char)WRITE_REG); else //now == true byteArray.append((char)WRITE_DATA); byteArray.append((char)GOAL_POSITION); //it transform's angle in degrees 0-300 to proper value in 2byte array angle from 0 to 1024 if(angle>300) angle=300; if(angle<0) angle=0; double intAngle = (double)angle*1023/(double)300; quint8 HB = intAngle/256; quint8 LB = intAngle-HB*256; byteArray.append((char)LB); byteArray.append((char)HB); addCRC(byteArray); send(byteArray); return true; }
//--end bool DynamixelServo::setSpeed(double Speed, bool broadcast) { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); if(broadcast) byteArray.append((char)BROADCAST); else byteArray.append((char)id); byteArray.append((char)0x05); byteArray.append((char)WRITE_DATA); byteArray.append((char)SPEED); //it transform's speed in percent to 2byte array speed in proper value 0 to 1024 if(Speed>100) Speed=100; if(Speed<0) Speed=0; double intSpeed = (double)Speed*1023/(double)100; quint8 HB = intSpeed/256; quint8 LB = intSpeed-HB*256; byteArray.append((char)LB); byteArray.append((char)HB); addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::turnLed(bool broadcast) { if(LedOn) LedOn = false; else LedOn = true; byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); if(broadcast) byteArray.append((char)BROADCAST); else byteArray.append((char)id); byteArray.append((char)0x04); byteArray.append((char)WRITE_DATA); byteArray.append((char)0x19); if(LedOn) byteArray.append((char)0x01); else byteArray.append((char)0x00); addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::setAlarmLeds(bool enable, bool broadcast) { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); if(broadcast) byteArray.append((char)BROADCAST); else byteArray.append((char)id); byteArray.append((char)0x04); byteArray.append((char)WRITE_DATA); byteArray.append((char)0x11); if(enable) byteArray.append((char)0x7f); else byteArray.append((char)0x00); addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::setMaxTorque(double torque) { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x05); byteArray.append((char)WRITE_DATA); byteArray.append((char)0x22); if(torque>100) torque=100; if(torque<0) torque=0; double intTorque = (double)torque*1023/(double)100; quint8 HB = intTorque/256; quint8 LB = intTorque-HB*256; byteArray.append((char)LB); byteArray.append((char)HB); addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::setDirectionAndSpeed(quint8 direction, double Speed, bool now) { if(direction !=0 && direction != 1) return false; byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x05); if(now) byteArray.append((char)WRITE_DATA); else byteArray.append((char)WRITE_REG); byteArray.append((char)SPEED); //it transform's speed in percent to 2byte array speed in proper value 0 to 1024 if(Speed>100) Speed=100; if(Speed<0) Speed=0; double intSpeed = (double)Speed*1023/(double)100 + direction*1024; //adding direction quint8 HB = intSpeed/256; quint8 LB = intSpeed-HB*256; byteArray.append((char)LB); byteArray.append((char)HB); addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::setCCWAngleLimit(double CCWAngle) //max 300 degree //counter clockwise { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x05); byteArray.append((char)WRITE_DATA); byteArray.append((char)CCW_ANGLE_LIMIT); //it transform's angle in degrees 0-300 to proper value in 2byte array angle from 0 to 1024 if(CCWAngle<0) CCWAngle = 0; if(CCWAngle>300) CCWAngle = 300; double intAngle = (double)CCWAngle*1023/(double)300; quint8 HB = intAngle/256; quint8 LB = intAngle-HB*256; byteArray.append((char)LB); byteArray.append((char)HB); this->CCWAngleLimitHB = HB; //needs for getting from wheel mode this->CCWAngleLimitLB = LB; addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::setAngleAndSpeed(double angle, double Speed, bool now) { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x07); if(now == false) //writes data to register, waits for function updateServo byteArray.append((char)WRITE_REG); else //now == true byteArray.append((char)WRITE_DATA); byteArray.append((char)GOAL_POSITION); ///setting angle //it transform's angle in degrees 0-300 to proper value in 2byte array angle from 0 to 1024 if(angle>300) angle=300; if(angle<0) angle=0; double intAngle = (double)angle*1023/(double)300; quint8 HB = intAngle/256; quint8 LB = intAngle-HB*256; byteArray.append((char)LB); byteArray.append((char)HB); ///setting speed //moving speed is register next after goal position, //so no need to say (can't say) where to write data if(Speed=100) Speed=100; if(Speed<0) Speed=0; double intSpeed = (double)Speed*1023/(double)100; HB = intSpeed/256; LB = intSpeed-HB*256; byteArray.append((char)LB); byteArray.append((char)HB); addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::updateServo(bool broadcast) //this or broadcast { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); if(broadcast) byteArray.append((char)BROADCAST); else byteArray.append((char)id); byteArray.append((char)0x02); byteArray.append((char)ACTION); addCRC(byteArray); send(byteArray); return true; }
void send_telegram() { tx_buffer.index = 0; addCRC(); Serial.write(FRAME_START_CHAR); // escape characters while (tx_buffer.index < tx_buffer.size) { for (uint8_t j = 0; j < number_char_to_excape; j++) { if (tx_buffer.data[tx_buffer.index] == escape_characters[j].toEscape) { Serial.write(escape_characters[j].howEscape[0]); tx_buffer.data[tx_buffer.index] = escape_characters[j].howEscape[1]; return; } } Serial.write(tx_buffer.data[tx_buffer.index++]); } Serial.write(FRAME_END_CHAR); }
bool DynamixelServo::getIfMoving(bool &moving) { //sending byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x04); byteArray.append((char)READ_DATA); byteArray.append((char)0x2e); byteArray.append((char)0x01); //how many bytes addCRC(byteArray); send(byteArray); //device->readLine(); ///TO BE FINISHED, prototype /* //getting QByteArray readByteArray = device->readAll(); //ff ff id length instr error param checksum byteArray.clear(); if(readByteArray.size() < ((quint8)readByteArray[3])+4) //not long enough return false; for(int i=0; i<readByteArray.size()-1; i++) //copying without CRC byteArray.append((char)((quint8)readByteArray[i])); addCRC(byteArray); if((quint8)byteArray[byteArray.size()-1] != (quint8)readByteArray[readByteArray.size()-1]) //if CRCs are not the same return false; if((quin8)byteArray[2] != this->id) return false; //if everything is fine if((quint8)readByteArray[readByteArray.size()-2] == 1) moving = true; else moving = false; */ return true; }
bool DynamixelServo::getSettedPosition(quint16 &value) { //sending byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); byteArray.append((char)id); byteArray.append((char)0x04); byteArray.append((char)READ_DATA); byteArray.append((char)GOAL_POSITION); byteArray.append((char)0x02); //how many bytes to read addCRC(byteArray); send(byteArray); ///TO BE FINISHED, prototype /* //getting QByteArray readByteArray = device->readAll(); //ff ff id length instr error param checksum byteArray.clear(); if(readByteArray.size() < ((quint8)readByteArray[3])+4) //not long enough return false; for(int i=0; i<readByteArray.size()-1; i++) //copying without CRC byteArray.append((char)((quint8)readByteArray[i])); addCRC(byteArray); if((quint8)byteArray[byteArray.size()-1] != (quint8)readByteArray[readByteArray.size()-1]) //if CRCs are not the same return false; if((quin8)byteArray[2] != this->id) return false; double valueTemp; //if everything is fine valueTemp = (quint8)readByteArray[readByteArray.size()-3]+ (quint8)readByteArray[readByteArray.size()-2]*256 + 1; valueTemp = (double)valueTemp/(double)1024*300; value = (quint16)valueTemp; */ return true;}
bool DynamixelServo::setTorqueOFF(bool broadcast) { byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); if(broadcast) byteArray.append((char)BROADCAST); else byteArray.append((char)id); byteArray.append((char)0x04); byteArray.append((char)WRITE_DATA); byteArray.append((char)0x18); //set - torque OFF byteArray.append((char)0x00); addCRC(byteArray); send(byteArray); return true; }
bool DynamixelServo::setStatusReturnPackage(quint8 info, bool broadcast) //sets if status package will be returned { if(info !=0 && info != 1 && info != 2) return false; byteArray.clear(); byteArray.append((char)0xff); byteArray.append((char)0xff); if(broadcast) byteArray.append((char)BROADCAST); else byteArray.append((char)id); byteArray.append((char)0x04); byteArray.append((char)WRITE_DATA); byteArray.append((char)0x10); //status package adress byteArray.append((char)info); addCRC(byteArray); send(byteArray); return true; }