test
Dependencies: MX28 iSerial mbed
Fork of MX_control by
classproject.cpp
- Committer:
- soulx
- Date:
- 2015-08-25
- Revision:
- 2:7f9d2f135591
- Parent:
- 1:0a652990dab1
File content as of revision 2:7f9d2f135591:
#include "mbed.h" #include "MX28.h" //config param //-motor robotis #define ID_ROLL 1 #define ID_PITCH 2 #define BANDRATE_MOTOR 1000000 //param calibration #define CENTER_ROLL 512 #define CENTER_PITCH 512 Serial pc(USBTX, USBRX); MX28 mx28(PA_11, PA_12, BANDRATE_MOTOR); DigitalIn mybutton(USER_BUTTON); //function prototype void setup_mode(); void run_mode(); int main() { pc.baud(115200); pc.getc(); pc.printf("======================================================\r\n"); uint8_t servoId = 0x01; uint16_t modelNumber; mx28.GetModelNumber(servoId, &modelNumber); uint8_t firmwareVersion; mx28.GetFirmwareVersion(servoId, &firmwareVersion); uint8_t id; mx28.GetId(servoId, &id); mx28.SetId(servoId, servoId); int32_t baudRate; mx28.GetBaudRate(servoId, &baudRate); mx28.SetBaudRate(servoId, 57600); uint8_t returnDelayTime; mx28.GetReturnDelayTime(servoId, &returnDelayTime); mx28.SetReturnDelayTime(servoId, 0xFA); uint16_t cwAngleLimit; mx28.GetCWAngleLimit(servoId, &cwAngleLimit); mx28.SetCWAngleLimit(servoId, 0x0000); uint16_t ccwAngleLimit; mx28.GetCCWAngleLimit(servoId, &ccwAngleLimit); mx28.SetCCWAngleLimit(servoId, 0x0FFF); uint8_t highestTemperatureLimit; mx28.GetHighestTemperatureLimit(servoId, &highestTemperatureLimit); mx28.SetHighestTemperatureLimit(servoId, 0x50); uint8_t downLimitVoltage; mx28.GetLowestVoltageLimit(servoId, &downLimitVoltage); mx28.SetLowestVoltageLimit(servoId, 0x3C); uint8_t upLimitVoltage; mx28.GetHighestVoltageLimit(servoId, &upLimitVoltage); mx28.SetHighestVoltageLimit(servoId, 0xA0); uint16_t maxTorque; mx28.GetMaxTorque(servoId, &maxTorque); mx28.SetMaxTorque(servoId, 0x03FF); uint8_t statusReturnLevel; mx28.GetStatusReturnLevel(servoId, &statusReturnLevel); mx28.SetStatusReturnLevel(servoId, 0x02); uint8_t alarmLED; mx28.GetAlarmLED(servoId, &alarmLED); mx28.SetAlarmLED(servoId, 0x24); uint8_t alarmShutdown; mx28.GetAlarmShutdown(servoId, &alarmShutdown); mx28.SetAlarmShutdown(servoId, 0x24); uint8_t enableTorque; mx28.GetEnableTorque(servoId, &enableTorque); mx28.SetEnableTorque(servoId, 0x00); uint8_t enableLED; mx28.GetEnableLED(servoId, &enableLED); mx28.SetEnableLED(servoId, 0x00); uint8_t pGain; mx28.GetPGain(servoId, &pGain); mx28.SetPGain(servoId, 0x20); uint8_t iGain; mx28.GetIGain(servoId, &iGain); mx28.SetIGain(servoId, 0x00); uint8_t dGain; mx28.GetDGain(servoId, &dGain); mx28.SetDGain(servoId, 0x00); uint16_t goalPosition; mx28.GetGoalPosition(servoId, &goalPosition); mx28.SetGoalPosition(servoId, 0x0800); uint16_t movingSpeed; mx28.GetMovingSpeed(servoId, &movingSpeed); mx28.SetMovingSpeed(servoId, 0x00FF); uint16_t torqueLimit; mx28.GetTorqueLimit(servoId, &torqueLimit); mx28.SetTorqueLimit(servoId, 0x03FF); uint16_t presentPosition; mx28.GetPresentPosition(servoId, &presentPosition); uint16_t presentSpeed; mx28.GetPresentSpeed(servoId, &presentSpeed); uint16_t presentLoad; mx28.GetPresentLoad(servoId, &presentLoad); uint8_t presentVoltage; mx28.GetPresentVoltage(servoId, &presentVoltage); uint8_t presentTemperature; mx28.GetPresentTemperature(servoId, &presentTemperature); uint8_t isRegistered; mx28.GetIsRegistered(servoId, &isRegistered); uint8_t isMoving; mx28.GetIsMoving(servoId, &isMoving); uint8_t lock; mx28.GetIsLocked(servoId, &lock); mx28.SetIsLocked(servoId, 0x00); uint16_t punch; mx28.GetPunch(servoId, &punch); mx28.SetPunch(servoId, 0x0020); mx28.Ping(servoId); mx28.Reset(servoId); uint8_t servo1Id = 0x01; uint8_t servo2Id = 0x02; uint8_t servo3Id = 0x03; uint16_t servo1GoalPosition = 0x0800; uint16_t servo2GoalPosition = 0x0800; uint16_t servo3GoalPosition = 0x0800; MX28_PROTOCOL_PACKET packet; packet.servoId = MX28_PROTOCOL_BROADCAST_ID; // (Data length + 1) * Number of servos + 4 packet.length = (2+ 1) * 3 + 4; packet.instructionErrorId = MX28_SYNC_WRITE; packet.parameter[0] = MX28_GOAL_POSITION_L; packet.parameter[1] = 0x06; packet.parameter[2] = servo1Id; Utilities::ConvertUInt16ToUInt8Array(servo1GoalPosition, (uint8_t*)&(packet.parameter[3])); packet.parameter[9] = servo2Id; Utilities::ConvertUInt16ToUInt8Array(servo2GoalPosition, (uint8_t*)&(packet.parameter[10])); packet.parameter[16] = servo3Id; Utilities::ConvertUInt16ToUInt8Array(servo3GoalPosition, (uint8_t*)&(packet.parameter[17])); pc.printf("Set servos goal positions: %hu %hu %hu\r\n", servo1GoalPosition, servo2GoalPosition, servo3GoalPosition); mx28.CommunicatePacket(&packet); packet.servoId = servoId; packet.length = 4; packet.instructionErrorId = MX28_READ_DATA; packet.parameter[0] = MX28_PRESENT_POSITION_L; packet.parameter[1] = 0x08; mx28.CommunicatePacket(&packet); presentPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter); presentSpeed = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[2])); presentLoad = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[4])); presentVoltage = packet.parameter[6]; presentTemperature = packet.parameter[7]; pc.printf("Present position: %hu\r\n", presentPosition); pc.printf("Present speed: %hu\r\n", presentSpeed); pc.printf("Present load: %hu\r\n", presentLoad); pc.printf("Present voltage: 0x%02X\r\n", presentVoltage); pc.printf("Present temperature: 0x%02X\r\n", presentTemperature); uint8_t status = mx28.GetModelNumber(servoId, &modelNumber); if(status == MX28_ERRBIT_WRITE_TIMEOUT) pc.printf("Error: Write timeout\r\n"); else if(status == MX28_ERRBIT_READ_TIMEOUT) pc.printf("Error: Read timeout\r\n"); else if(status == MX28_ERRBIT_MASTER_CHECKSUM) pc.printf("Error: Master checksum error\r\n"); else { if(status & MX28_ERRBIT_VOLTAGE) pc.printf("Error: Input voltage error\r\n"); if(status & MX28_ERRBIT_ANGLE) pc.printf("Error: Angle limit error\r\n"); if(status & MX28_ERRBIT_OVERHEAT) pc.printf("Error: Overheat error\r\n"); if(status & MX28_ERRBIT_RANGE) pc.printf("Error: Out of range error\r\n"); if(status & MX28_ERRBIT_CHECKSUM) pc.printf("Error: Checksum error\r\n"); if(status & MX28_ERRBIT_OVERLOAD) pc.printf("Error: Overload error\r\n"); if(status & MX28_ERRBIT_INSTRUCTION) pc.printf("Error: Instruction code error\r\n"); } pc.printf("======================================================\r\n"); return 0; } /* int main() { pc.baud(115200); pc.printf("======================================================\r\n"); if(mybutton == 0) { //Setup mode pc.printf("Setup Mode\r\n"); setup_mode(); } else { //Run mode pc.printf("Run Mode\r\n"); run_mode(); } return 0; } void setup_mode() { } void run_mode() { } */