BE@R lab / Mbed 2 deprecated MX_control

test

Dependencies:   MX28 iSerial mbed

Fork of MX_control by FIBO

classproject.cpp@2:7f9d2f135591, 2015-08-25 (annotated)

Committer:
soulx
Date:
Tue Aug 25 08:53:41 2015 +0000
Revision:
2:7f9d2f135591
Parent:
1:0a652990dab1 
test_motor robotic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soulx 0:c98ac0a05620 1 #include "mbed.h"
soulx 0:c98ac0a05620 2 #include "MX28.h"
soulx 0:c98ac0a05620 3
soulx 1:0a652990dab1 4 //config param
soulx 1:0a652990dab1 5 //-motor robotis
soulx 0:c98ac0a05620 6 #define ID_ROLL 1
soulx 0:c98ac0a05620 7 #define ID_PITCH 2
soulx 1:0a652990dab1 8 #define BANDRATE_MOTOR 1000000
soulx 0:c98ac0a05620 9
soulx 1:0a652990dab1 10
soulx 1:0a652990dab1 11 //param calibration
soulx 1:0a652990dab1 12 #define CENTER_ROLL 512
soulx 1:0a652990dab1 13 #define CENTER_PITCH 512
soulx 1:0a652990dab1 14
soulx 1:0a652990dab1 15
soulx 0:c98ac0a05620 16
soulx 0:c98ac0a05620 17 Serial pc(USBTX, USBRX);
soulx 1:0a652990dab1 18 MX28 mx28(PA_11, PA_12, BANDRATE_MOTOR);
soulx 1:0a652990dab1 19
soulx 1:0a652990dab1 20 DigitalIn mybutton(USER_BUTTON);
soulx 1:0a652990dab1 21
soulx 1:0a652990dab1 22 //function prototype
soulx 1:0a652990dab1 23 void setup_mode();
soulx 1:0a652990dab1 24 void run_mode();
soulx 1:0a652990dab1 25
soulx 0:c98ac0a05620 26
soulx 2:7f9d2f135591 27 int main()
soulx 2:7f9d2f135591 28 {
soulx 2:7f9d2f135591 29 pc.baud(115200);
soulx 2:7f9d2f135591 30
soulx 2:7f9d2f135591 31 pc.getc();
soulx 2:7f9d2f135591 32 pc.printf("======================================================\r\n");
soulx 2:7f9d2f135591 33
soulx 2:7f9d2f135591 34 uint8_t servoId = 0x01;
soulx 2:7f9d2f135591 35
soulx 2:7f9d2f135591 36 uint16_t modelNumber;
soulx 2:7f9d2f135591 37 mx28.GetModelNumber(servoId, &modelNumber);
soulx 2:7f9d2f135591 38
soulx 2:7f9d2f135591 39 uint8_t firmwareVersion;
soulx 2:7f9d2f135591 40 mx28.GetFirmwareVersion(servoId, &firmwareVersion);
soulx 2:7f9d2f135591 41
soulx 2:7f9d2f135591 42 uint8_t id;
soulx 2:7f9d2f135591 43 mx28.GetId(servoId, &id);
soulx 2:7f9d2f135591 44 mx28.SetId(servoId, servoId);
soulx 2:7f9d2f135591 45
soulx 2:7f9d2f135591 46 int32_t baudRate;
soulx 2:7f9d2f135591 47 mx28.GetBaudRate(servoId, &baudRate);
soulx 2:7f9d2f135591 48 mx28.SetBaudRate(servoId, 57600);
soulx 2:7f9d2f135591 49
soulx 2:7f9d2f135591 50 uint8_t returnDelayTime;
soulx 2:7f9d2f135591 51 mx28.GetReturnDelayTime(servoId, &returnDelayTime);
soulx 2:7f9d2f135591 52 mx28.SetReturnDelayTime(servoId, 0xFA);
soulx 2:7f9d2f135591 53
soulx 2:7f9d2f135591 54 uint16_t cwAngleLimit;
soulx 2:7f9d2f135591 55 mx28.GetCWAngleLimit(servoId, &cwAngleLimit);
soulx 2:7f9d2f135591 56 mx28.SetCWAngleLimit(servoId, 0x0000);
soulx 2:7f9d2f135591 57
soulx 2:7f9d2f135591 58 uint16_t ccwAngleLimit;
soulx 2:7f9d2f135591 59 mx28.GetCCWAngleLimit(servoId, &ccwAngleLimit);
soulx 2:7f9d2f135591 60 mx28.SetCCWAngleLimit(servoId, 0x0FFF);
soulx 2:7f9d2f135591 61
soulx 2:7f9d2f135591 62 uint8_t highestTemperatureLimit;
soulx 2:7f9d2f135591 63 mx28.GetHighestTemperatureLimit(servoId, &highestTemperatureLimit);
soulx 2:7f9d2f135591 64 mx28.SetHighestTemperatureLimit(servoId, 0x50);
soulx 2:7f9d2f135591 65
soulx 2:7f9d2f135591 66 uint8_t downLimitVoltage;
soulx 2:7f9d2f135591 67 mx28.GetLowestVoltageLimit(servoId, &downLimitVoltage);
soulx 2:7f9d2f135591 68 mx28.SetLowestVoltageLimit(servoId, 0x3C);
soulx 2:7f9d2f135591 69
soulx 2:7f9d2f135591 70 uint8_t upLimitVoltage;
soulx 2:7f9d2f135591 71 mx28.GetHighestVoltageLimit(servoId, &upLimitVoltage);
soulx 2:7f9d2f135591 72 mx28.SetHighestVoltageLimit(servoId, 0xA0);
soulx 2:7f9d2f135591 73
soulx 2:7f9d2f135591 74 uint16_t maxTorque;
soulx 2:7f9d2f135591 75 mx28.GetMaxTorque(servoId, &maxTorque);
soulx 2:7f9d2f135591 76 mx28.SetMaxTorque(servoId, 0x03FF);
soulx 2:7f9d2f135591 77
soulx 2:7f9d2f135591 78 uint8_t statusReturnLevel;
soulx 2:7f9d2f135591 79 mx28.GetStatusReturnLevel(servoId, &statusReturnLevel);
soulx 2:7f9d2f135591 80 mx28.SetStatusReturnLevel(servoId, 0x02);
soulx 2:7f9d2f135591 81
soulx 2:7f9d2f135591 82 uint8_t alarmLED;
soulx 2:7f9d2f135591 83 mx28.GetAlarmLED(servoId, &alarmLED);
soulx 2:7f9d2f135591 84 mx28.SetAlarmLED(servoId, 0x24);
soulx 2:7f9d2f135591 85
soulx 2:7f9d2f135591 86 uint8_t alarmShutdown;
soulx 2:7f9d2f135591 87 mx28.GetAlarmShutdown(servoId, &alarmShutdown);
soulx 2:7f9d2f135591 88 mx28.SetAlarmShutdown(servoId, 0x24);
soulx 2:7f9d2f135591 89
soulx 2:7f9d2f135591 90 uint8_t enableTorque;
soulx 2:7f9d2f135591 91 mx28.GetEnableTorque(servoId, &enableTorque);
soulx 2:7f9d2f135591 92 mx28.SetEnableTorque(servoId, 0x00);
soulx 2:7f9d2f135591 93
soulx 2:7f9d2f135591 94 uint8_t enableLED;
soulx 2:7f9d2f135591 95 mx28.GetEnableLED(servoId, &enableLED);
soulx 2:7f9d2f135591 96 mx28.SetEnableLED(servoId, 0x00);
soulx 2:7f9d2f135591 97
soulx 2:7f9d2f135591 98 uint8_t pGain;
soulx 2:7f9d2f135591 99 mx28.GetPGain(servoId, &pGain);
soulx 2:7f9d2f135591 100 mx28.SetPGain(servoId, 0x20);
soulx 2:7f9d2f135591 101
soulx 2:7f9d2f135591 102 uint8_t iGain;
soulx 2:7f9d2f135591 103 mx28.GetIGain(servoId, &iGain);
soulx 2:7f9d2f135591 104 mx28.SetIGain(servoId, 0x00);
soulx 2:7f9d2f135591 105
soulx 2:7f9d2f135591 106 uint8_t dGain;
soulx 2:7f9d2f135591 107 mx28.GetDGain(servoId, &dGain);
soulx 2:7f9d2f135591 108 mx28.SetDGain(servoId, 0x00);
soulx 2:7f9d2f135591 109
soulx 2:7f9d2f135591 110 uint16_t goalPosition;
soulx 2:7f9d2f135591 111 mx28.GetGoalPosition(servoId, &goalPosition);
soulx 2:7f9d2f135591 112 mx28.SetGoalPosition(servoId, 0x0800);
soulx 2:7f9d2f135591 113
soulx 2:7f9d2f135591 114 uint16_t movingSpeed;
soulx 2:7f9d2f135591 115 mx28.GetMovingSpeed(servoId, &movingSpeed);
soulx 2:7f9d2f135591 116 mx28.SetMovingSpeed(servoId, 0x00FF);
soulx 2:7f9d2f135591 117
soulx 2:7f9d2f135591 118 uint16_t torqueLimit;
soulx 2:7f9d2f135591 119 mx28.GetTorqueLimit(servoId, &torqueLimit);
soulx 2:7f9d2f135591 120 mx28.SetTorqueLimit(servoId, 0x03FF);
soulx 2:7f9d2f135591 121
soulx 2:7f9d2f135591 122 uint16_t presentPosition;
soulx 2:7f9d2f135591 123 mx28.GetPresentPosition(servoId, &presentPosition);
soulx 2:7f9d2f135591 124
soulx 2:7f9d2f135591 125 uint16_t presentSpeed;
soulx 2:7f9d2f135591 126 mx28.GetPresentSpeed(servoId, &presentSpeed);
soulx 2:7f9d2f135591 127
soulx 2:7f9d2f135591 128 uint16_t presentLoad;
soulx 2:7f9d2f135591 129 mx28.GetPresentLoad(servoId, &presentLoad);
soulx 2:7f9d2f135591 130
soulx 2:7f9d2f135591 131 uint8_t presentVoltage;
soulx 2:7f9d2f135591 132 mx28.GetPresentVoltage(servoId, &presentVoltage);
soulx 2:7f9d2f135591 133
soulx 2:7f9d2f135591 134 uint8_t presentTemperature;
soulx 2:7f9d2f135591 135 mx28.GetPresentTemperature(servoId, &presentTemperature);
soulx 2:7f9d2f135591 136
soulx 2:7f9d2f135591 137 uint8_t isRegistered;
soulx 2:7f9d2f135591 138
soulx 2:7f9d2f135591 139 mx28.GetIsRegistered(servoId, &isRegistered);
soulx 2:7f9d2f135591 140
soulx 2:7f9d2f135591 141 uint8_t isMoving;
soulx 2:7f9d2f135591 142 mx28.GetIsMoving(servoId, &isMoving);
soulx 2:7f9d2f135591 143
soulx 2:7f9d2f135591 144 uint8_t lock;
soulx 2:7f9d2f135591 145 mx28.GetIsLocked(servoId, &lock);
soulx 2:7f9d2f135591 146 mx28.SetIsLocked(servoId, 0x00);
soulx 2:7f9d2f135591 147
soulx 2:7f9d2f135591 148 uint16_t punch;
soulx 2:7f9d2f135591 149 mx28.GetPunch(servoId, &punch);
soulx 2:7f9d2f135591 150 mx28.SetPunch(servoId, 0x0020);
soulx 2:7f9d2f135591 151
soulx 2:7f9d2f135591 152 mx28.Ping(servoId);
soulx 2:7f9d2f135591 153
soulx 2:7f9d2f135591 154 mx28.Reset(servoId);
soulx 2:7f9d2f135591 155
soulx 2:7f9d2f135591 156 uint8_t servo1Id = 0x01;
soulx 2:7f9d2f135591 157 uint8_t servo2Id = 0x02;
soulx 2:7f9d2f135591 158 uint8_t servo3Id = 0x03;
soulx 2:7f9d2f135591 159
soulx 2:7f9d2f135591 160 uint16_t servo1GoalPosition = 0x0800;
soulx 2:7f9d2f135591 161 uint16_t servo2GoalPosition = 0x0800;
soulx 2:7f9d2f135591 162 uint16_t servo3GoalPosition = 0x0800;
soulx 2:7f9d2f135591 163
soulx 2:7f9d2f135591 164 MX28_PROTOCOL_PACKET packet;
soulx 2:7f9d2f135591 165 packet.servoId = MX28_PROTOCOL_BROADCAST_ID;
soulx 2:7f9d2f135591 166 // (Data length + 1) * Number of servos + 4
soulx 2:7f9d2f135591 167 packet.length = (2+ 1) * 3 + 4;
soulx 2:7f9d2f135591 168 packet.instructionErrorId = MX28_SYNC_WRITE;
soulx 2:7f9d2f135591 169 packet.parameter[0] = MX28_GOAL_POSITION_L;
soulx 2:7f9d2f135591 170 packet.parameter[1] = 0x06;
soulx 2:7f9d2f135591 171 packet.parameter[2] = servo1Id;
soulx 2:7f9d2f135591 172 Utilities::ConvertUInt16ToUInt8Array(servo1GoalPosition, (uint8_t*)&(packet.parameter[3]));
soulx 2:7f9d2f135591 173 packet.parameter[9] = servo2Id;
soulx 2:7f9d2f135591 174 Utilities::ConvertUInt16ToUInt8Array(servo2GoalPosition, (uint8_t*)&(packet.parameter[10]));
soulx 2:7f9d2f135591 175 packet.parameter[16] = servo3Id;
soulx 2:7f9d2f135591 176 Utilities::ConvertUInt16ToUInt8Array(servo3GoalPosition, (uint8_t*)&(packet.parameter[17]));
soulx 2:7f9d2f135591 177
soulx 2:7f9d2f135591 178 pc.printf("Set servos goal positions: %hu %hu %hu\r\n", servo1GoalPosition, servo2GoalPosition, servo3GoalPosition);
soulx 2:7f9d2f135591 179
soulx 2:7f9d2f135591 180 mx28.CommunicatePacket(&packet);
soulx 2:7f9d2f135591 181
soulx 2:7f9d2f135591 182 packet.servoId = servoId;
soulx 2:7f9d2f135591 183 packet.length = 4;
soulx 2:7f9d2f135591 184 packet.instructionErrorId = MX28_READ_DATA;
soulx 2:7f9d2f135591 185 packet.parameter[0] = MX28_PRESENT_POSITION_L;
soulx 2:7f9d2f135591 186 packet.parameter[1] = 0x08;
soulx 2:7f9d2f135591 187
soulx 2:7f9d2f135591 188 mx28.CommunicatePacket(&packet);
soulx 2:7f9d2f135591 189
soulx 2:7f9d2f135591 190 presentPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
soulx 2:7f9d2f135591 191 presentSpeed = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[2]));
soulx 2:7f9d2f135591 192 presentLoad = Utilities::ConvertUInt8ArrayToUInt16((uint8_t*)&(packet.parameter[4]));
soulx 2:7f9d2f135591 193 presentVoltage = packet.parameter[6];
soulx 2:7f9d2f135591 194 presentTemperature = packet.parameter[7];
soulx 2:7f9d2f135591 195
soulx 2:7f9d2f135591 196 pc.printf("Present position: %hu\r\n", presentPosition);
soulx 2:7f9d2f135591 197 pc.printf("Present speed: %hu\r\n", presentSpeed);
soulx 2:7f9d2f135591 198 pc.printf("Present load: %hu\r\n", presentLoad);
soulx 2:7f9d2f135591 199 pc.printf("Present voltage: 0x%02X\r\n", presentVoltage);
soulx 2:7f9d2f135591 200 pc.printf("Present temperature: 0x%02X\r\n", presentTemperature);
soulx 2:7f9d2f135591 201
soulx 2:7f9d2f135591 202 uint8_t status = mx28.GetModelNumber(servoId, &modelNumber);
soulx 2:7f9d2f135591 203
soulx 2:7f9d2f135591 204 if(status == MX28_ERRBIT_WRITE_TIMEOUT)
soulx 2:7f9d2f135591 205 pc.printf("Error: Write timeout\r\n");
soulx 2:7f9d2f135591 206 else if(status == MX28_ERRBIT_READ_TIMEOUT)
soulx 2:7f9d2f135591 207 pc.printf("Error: Read timeout\r\n");
soulx 2:7f9d2f135591 208 else if(status == MX28_ERRBIT_MASTER_CHECKSUM)
soulx 2:7f9d2f135591 209 pc.printf("Error: Master checksum error\r\n");
soulx 2:7f9d2f135591 210 else
soulx 2:7f9d2f135591 211 {
soulx 2:7f9d2f135591 212 if(status & MX28_ERRBIT_VOLTAGE)
soulx 2:7f9d2f135591 213 pc.printf("Error: Input voltage error\r\n");
soulx 2:7f9d2f135591 214 if(status & MX28_ERRBIT_ANGLE)
soulx 2:7f9d2f135591 215 pc.printf("Error: Angle limit error\r\n");
soulx 2:7f9d2f135591 216 if(status & MX28_ERRBIT_OVERHEAT)
soulx 2:7f9d2f135591 217 pc.printf("Error: Overheat error\r\n");
soulx 2:7f9d2f135591 218 if(status & MX28_ERRBIT_RANGE)
soulx 2:7f9d2f135591 219 pc.printf("Error: Out of range error\r\n");
soulx 2:7f9d2f135591 220 if(status & MX28_ERRBIT_CHECKSUM)
soulx 2:7f9d2f135591 221 pc.printf("Error: Checksum error\r\n");
soulx 2:7f9d2f135591 222 if(status & MX28_ERRBIT_OVERLOAD)
soulx 2:7f9d2f135591 223 pc.printf("Error: Overload error\r\n");
soulx 2:7f9d2f135591 224 if(status & MX28_ERRBIT_INSTRUCTION)
soulx 2:7f9d2f135591 225 pc.printf("Error: Instruction code error\r\n");
soulx 2:7f9d2f135591 226 }
soulx 2:7f9d2f135591 227
soulx 2:7f9d2f135591 228 pc.printf("======================================================\r\n");
soulx 2:7f9d2f135591 229
soulx 2:7f9d2f135591 230 return 0;
soulx 2:7f9d2f135591 231 }
soulx 2:7f9d2f135591 232 /*
soulx 0:c98ac0a05620 233 int main()
soulx 0:c98ac0a05620 234 {
soulx 2:7f9d2f135591 235 pc.baud(115200);
soulx 2:7f9d2f135591 236 pc.printf("======================================================\r\n");
soulx 2:7f9d2f135591 237 if(mybutton == 0) {
soulx 2:7f9d2f135591 238 //Setup mode
soulx 2:7f9d2f135591 239 pc.printf("Setup Mode\r\n");
soulx 2:7f9d2f135591 240 setup_mode();
soulx 2:7f9d2f135591 241 } else {
soulx 2:7f9d2f135591 242 //Run mode
soulx 1:0a652990dab1 243 pc.printf("Run Mode\r\n");
soulx 1:0a652990dab1 244 run_mode();
soulx 1:0a652990dab1 245 }
soulx 2:7f9d2f135591 246 return 0;
soulx 1:0a652990dab1 247 }
soulx 1:0a652990dab1 248
soulx 1:0a652990dab1 249 void setup_mode()
soulx 1:0a652990dab1 250 {
soulx 2:7f9d2f135591 251
soulx 1:0a652990dab1 252 }
soulx 1:0a652990dab1 253
soulx 1:0a652990dab1 254 void run_mode()
soulx 1:0a652990dab1 255 {
soulx 2:7f9d2f135591 256
soulx 2:7f9d2f135591 257 }
soulx 2:7f9d2f135591 258 */