Speedy Retraction Team
/
Nucleo_Dynamixel_18V
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Dynamixel Protocol Utilities mbed
Fork of
Nucleo_Dynamixel_18V
by 
Timofey Ilin
main.cpp
- Committer:
- Mirthilon
- Date:
- 2016-07-10
- Revision:
- 6:414eb29191ad
- Parent:
- 5:c74f23e4325f
- Child:
- 7:761930a5373d
File content as of revision 6:414eb29191ad:
/* Tilt MX-28 ID: 0x01
Test RX-28 ID: 0x02 (DMXL04)
Wrist RX-28 ID: 0x03 (DMXL05)
Pan MX-28 ID: 0x04
Elbow RX-28 ID: 0x05 (DMXL06)
Base RX-28 ID: 0x06 (DMXL08)
//DON'T GO TO ZERO ON BASE SERVO
*/
#include "mbed.h"
#include "Dynamixel.h"
Serial pc(USBTX,USBRX);
Dynamixel Base(PA_9, PA_10, PB_3, 6, 57600); // RX-28
Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600); // RX-28
Dynamixel Wrist(PA_9, PA_10, PB_3, 2, 57600); // RX-28
Dynamixel Pan(PA_9, PA_10, PB_3, 4, 57600); // MX-28
Dynamixel Tilt(PA_9, PA_10, PB_3, 1, 57600); // MX-28
DigitalOut myled(LED1);
uint8_t pos = 1;
uint16_t tiltInc = 50; // was 15 for RX
uint16_t panInc = 50;
int echo;
void centrePosition() {
if (pos == 1) {
Wrist.setMovingSpeed(1024+1.4*100); // SPEEEEEEEED
Base.setMovingSpeed(1024+100); //Speeed
Elbow.setMovingSpeed(1024+2.3*100); //Speeed
}
else if (pos == 3) {
Wrist.setMovingSpeed(1024+150); // SPEEEEEEEED
Base.setMovingSpeed(1024+50); //Speeed
Elbow.setMovingSpeed(1024+100); //Speeed
}
else if (pos == 4) {
Wrist.setMovingSpeed(1024+70); // SPEEEEEEEED
Base.setMovingSpeed(1024+100); //Speeed
Elbow.setMovingSpeed(1024+50); //Speeed
}
Base.setGoalPosition(150);
Elbow.setGoalPosition(150);
Wrist.setGoalPosition(180);
pos = 2;
wait(1);
}
void leftPosition() {
uint16_t currentPosBase;
uint16_t currentPosElbow;
Wrist.setMovingSpeed(1024+1.5*100); // SPEEEEEEEED
if (pos == 1) {
centrePosition();
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
while(currentPosBase < 140 && currentPosElbow > 160) {
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
}
}
Wrist.setMovingSpeed(1024+80);
if (pos == 4) {
Wrist.setMovingSpeed(1024+50);
}
// SPEEEEEEEED
Elbow.setMovingSpeed(1024+50); //Speeed
Base.setMovingSpeed(1024+30); //Speeed
Base.setGoalPosition(70);
Elbow.setGoalPosition(60);
Wrist.setGoalPosition(370);
pos = 3;
wait(1);
}
void rightPosition() {
uint16_t currentPosBase;
uint16_t currentPosElbow;
Wrist.setMovingSpeed(1024+1.5*100); // SPEEEEEEEED
if (pos == 1) {
centrePosition();
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
while(currentPosBase < 145 && currentPosElbow > 155) {
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
}
}
if (pos == 3) {
//Base.setGoalPosition(150); // To lessen lever disadvantage
Elbow.setGoalPosition(150);
//Wrist.setMovingSpeed(1024+50);
//Wrist.setGoalPosition(140);
wait(1);
}
Wrist.setMovingSpeed(1024+110); // SPEEEEEEEED
Elbow.setMovingSpeed(1024+50); //Speeed
Base.setMovingSpeed(1024+100); //Speeed
Base.setGoalPosition(350);
Elbow.setGoalPosition(50);
wait_us(100);
Wrist.setGoalPosition(60);
pos = 4;
wait(1);
}
void retractedPosition() {
uint16_t currentPosBase;
uint16_t currentPosElbow;
Wrist.setMovingSpeed(1024+100); // SPEEEEEEEED
Base.setMovingSpeed(1024+100);
if (pos == 3) {
centrePosition();
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
while(currentPosBase < 155 && currentPosElbow < 145) {
currentPosBase = Base.getPresentPosition();
}
Base.setMovingSpeed(1024+120);
}
else if (pos == 4) {
centrePosition();
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
while(currentPosBase > 155 && currentPosElbow < 145) {
currentPosBase = Base.getPresentPosition();
}
}
Wrist.setMovingSpeed(1024+1.4*100); // SPEEEEEEEED
Elbow.setMovingSpeed(1024+2.3*100); //Speeed
//if (pos == 3) {
//}
Base.setGoalPosition(60);
Elbow.setGoalPosition(350);
Wrist.setGoalPosition(60);
pos = 1;
wait(1);
}
void measureTime() {
uint16_t currentPosBase;
Timer t;
uint32_t time;
t.start();
currentPosBase = Base.getPresentPosition();
while(currentPosBase < 1280) {
currentPosBase = Base.getPresentPosition();
}
time = t.read_ms();
pc.printf("Time: %d\r\n",time);
t.stop();
t.reset();
}
void setSpeed(uint16_t speed) {
Base.setMovingSpeed(1024+speed);
Elbow.setMovingSpeed(1024+2.3*speed); // was 1024+2.2*speed
Wrist.setMovingSpeed(1024+1.4*speed);
//Pan.setMovingSpeed(1024+0.3*speed);
//Tilt.setMovingSpeed(1024+0.3*speed);
}
void setLimits() {
Base.setCWAngleLimit(300);
Elbow.setCWAngleLimit(50);
Base.setCCWAngleLimit(1300);
Elbow.setCCWAngleLimit(365);
Tilt.setCWAngleLimit(210);
Tilt.setCCWAngleLimit(1000);
}
void tiltIncrementUp() {
uint16_t currentPosTilt;
currentPosTilt = Tilt.getPresentPosition();
Tilt.setGoalPosition(currentPosTilt-tiltInc);
wait_ms(10);
}
void tiltIncrementDown() {
uint16_t currentPosTilt;
currentPosTilt = Tilt.getPresentPosition();
Tilt.setGoalPosition(currentPosTilt+tiltInc);
wait_ms(10);
}
void panIncrementLeft() {
uint16_t currentPosPan;
currentPosPan = Pan.getPresentPosition();
Pan.setGoalPosition(currentPosPan+panInc);
wait_ms(10);
/*
currentPosPan = Pan.getPresentPosition();
Pan.setMovingSpeed(1124);
Pan.setGoalPosition(2000); //4095
*/
}
void panIncrementRight() {
uint16_t currentPosPan;
currentPosPan = Pan.getPresentPosition();
Pan.setGoalPosition(currentPosPan-panInc);
wait_ms(10);
/*
currentPosPan = Pan.getPresentPosition();
Pan.setMovingSpeed(1124);
Pan.setGoalPosition(500); //0
*/
}
int main()
{
pc.baud(9600);
setSpeed(100);
uint16_t currentPosTilt;
uint16_t currentPosPan;
//Pan.setCWAngleLimit(0);
//Pan.setCCWAngleLimit(4095);
//Pan.setGoalPosition(1850);
//Tilt.setGoalPosition(195);
Base.setGoalPosition(60);
Elbow.setGoalPosition(350);
Wrist.setGoalPosition(50);
while(1) {
//if (echo == 0) {
//Base.setGoalPosition(1300);
//Elbow.setGoalPosition(365);
//}
echo = pc.getc();
//pc.printf("\r\n %d \r\n", echo);
pc.putc(echo);
if (echo == 49) { retractedPosition(); }
else if (echo == 50) { centrePosition(); }
else if (echo == 51) { leftPosition(); }
else if (echo == 52) { rightPosition(); }
if (echo == 119) { tiltIncrementUp(); }
else if (echo == 115) { tiltIncrementDown(); }
else if (echo == 97) { panIncrementLeft(); }
else if (echo == 100) { panIncrementRight(); }
else {
continue;
//Tilt.setMovingSpeed(1024);
//Pan.setMovingSpeed(1024);
//currentPosPan = Pan.getPresentPosition();
//Pan.setGoalPosition(currentPosPan);
}
//currentPosTilt = Tilt.getPresentPosition();
//pc.printf("Tilt Position: %d\r\n",currentPosTilt);
//currentPosPan = Pan.getPresentPosition();
//pc.printf("Pan Position: %d\r\n",currentPosPan);
/*
retractedPosition();
wait(3);
leftPosition();
wait(3);
rightPosition();
wait(3);
centrePosition();
wait(3);
retractedPosition();
wait(3);
rightPosition();
wait(3);
centrePosition();
wait(3);
leftPosition();
wait(3);*/
}
}
/*//MX28a.reset();
MX28a.setId(6);
//wait(1);
MX28a.setEnableLED(1);
//RX28a.setEnableLED(0);
wait(1);
MX28a.setEnableLED(0);
//RX28a.setEnableLED(1);
wait(1);
MX28a.setGoalPosition(0);
RX28a.setGoalPosition(1023);
wait(1);
MX28a.setGoalPosition(4095);
RX28a.setGoalPosition(0);
wait(1);
delayTest = MX28a.getReturnDelayTime();
pc.printf("Delay Time: 0x%04x\r\n",delayTest);
wait(1);
modelNumber = MX28a.getModelNumber();
pc.printf("Model Number: 0x%04x\r\n",modelNumber);
wait(1);
id = MX28a.getId();
pc.printf("ID Number: 0x%04x\r\n",id);
if (id == 0x01) {
myled = !myled;
}
wait(1);
baud = MX28a.getBaudRate();
pc.printf("Baud Rate: %d\r\n",baud);
*/
/*int rar = 1;
int goalPos = 60;
int Xm = 0;
int Ym = 0;
int Xe = 0;
int Ye = 0;
int L = 147; // mm
int S = 97; // mm
int thet = 0;
int gamm = 0;*/
/*currentPosBase = Base.getPresentPosition();
pc.printf("Base Current Position: %d\r\n",currentPosBase);
currentPosElbow = Elbow.getPresentPosition();
pc.printf("Elbow Current Position: %d\r\n",currentPosElbow);
//wait(3);
if (currentPosElbow == 360 && rar == 1) {
rar = -1;
}
else if (currentPosElbow == 60 && rar == -1) {
rar = 1;
}
Elbow.setGoalPosition(goalPos);
goalPos += rar;*/
//error = Base.setGoalPosition(0);
//Elbow.setGoalPosition(0);
//pc.printf("Error: 0x%04x\r\n",error);
//wait(1);