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
Diff: main.cpp
- Revision:
- 4:0c1555b01361
- Parent:
- 3:09a27cf1c9fe
- Child:
- 5:c74f23e4325f
--- a/main.cpp Wed Jul 06 02:19:02 2016 +0000
+++ b/main.cpp Fri Jul 08 08:54:39 2016 +0000
@@ -1,28 +1,48 @@
-/* Test MX-28 ID: 0x01
- Pan RX-28 ID: 0x02 (DMXL04)
- Tilt RX-28 ID: 0x03 (DMXL05)
- Base MX-28 ID: 0x04
+/* 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)
- Wrist RX-28 ID: 0x06 (DMXL08)
+ 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, 1, 57600);
-Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600);
-Dynamixel Pan(PA_9, PA_10, PB_3, 4, 57600);
-Dynamixel Tilt(PA_9, PA_10, PB_3, 3, 57600);
+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;
-uint16_t tiltInc = 15;
-uint16_t panInc = 60;
+uint8_t pos = 1;
+uint16_t tiltInc = 100; // was 15 for RX
+uint16_t panInc = 100;
int echo;
void centrePosition() {
- Base.setGoalPosition(800);
- Elbow.setGoalPosition(50);
+ 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(200);
pos = 2;
wait(2);
}
@@ -30,18 +50,24 @@
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 > 820 && currentPosElbow > 70) {
+ while(currentPosBase < 145 && currentPosElbow > 155) {
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
}
}
- Base.setGoalPosition(300);
- Elbow.setGoalPosition(50);
+ Wrist.setMovingSpeed(1024+80); // SPEEEEEEEED
+ Elbow.setMovingSpeed(1024+50); //Speeed
+ Base.setMovingSpeed(1024+30); //Speeed
+
+ Base.setGoalPosition(70);
+ Elbow.setGoalPosition(60);
+ Wrist.setGoalPosition(400);
pos = 3;
wait(2);
}
@@ -49,41 +75,63 @@
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 > 820 && currentPosElbow > 70) {
+ while(currentPosBase < 145 && currentPosElbow > 155) {
currentPosBase = Base.getPresentPosition();
currentPosElbow = Elbow.getPresentPosition();
}
}
- Base.setGoalPosition(1300);
+ if (pos == 3) {
+ Base.setGoalPosition(150); // To lessen lever disadvantage
+ wait(0.5);
+ }
+
+ Wrist.setMovingSpeed(1024+70); // SPEEEEEEEED
+ Elbow.setMovingSpeed(1024+50); //Speeed
+ Base.setMovingSpeed(1024+100); //Speeed
+ Base.setGoalPosition(350);
Elbow.setGoalPosition(50);
+ Wrist.setGoalPosition(60);
pos = 4;
wait(2);
}
void retractedPosition() {
uint16_t currentPosBase;
+ uint16_t currentPosElbow;
+ Wrist.setMovingSpeed(1024+100); // SPEEEEEEEED
if (pos == 3) {
centrePosition();
currentPosBase = Base.getPresentPosition();
- while(currentPosBase < 780) {
+ currentPosElbow = Elbow.getPresentPosition();
+ while(currentPosBase < 155 && currentPosElbow < 145) {
currentPosBase = Base.getPresentPosition();
}
+ Base.setMovingSpeed(1024+120);
}
else if (pos == 4) {
centrePosition();
currentPosBase = Base.getPresentPosition();
- while(currentPosBase > 820) {
+ currentPosElbow = Elbow.getPresentPosition();
+ while(currentPosBase > 155 && currentPosElbow < 145) {
currentPosBase = Base.getPresentPosition();
}
}
- Base.setGoalPosition(1300);
- Elbow.setGoalPosition(365);
+ Wrist.setMovingSpeed(1024+1.4*100); // SPEEEEEEEED
+ Elbow.setMovingSpeed(1024+2.3*100); //Speeed
+ //if (pos == 3) {
+
+ //}
+
+ Base.setGoalPosition(60);
+ Elbow.setGoalPosition(360);
+ Wrist.setGoalPosition(65);
pos = 1;
wait(2);
}
@@ -106,7 +154,8 @@
void setSpeed(uint16_t speed) {
Base.setMovingSpeed(1024+speed);
- Elbow.setMovingSpeed(1024+2.2*speed);
+ Elbow.setMovingSpeed(1024+2.3*speed); // was 1024+2.2*speed
+ Wrist.setMovingSpeed(1024+1.4*speed);
}
void setLimits() {
@@ -122,7 +171,7 @@
uint16_t currentPosTilt;
currentPosTilt = Tilt.getPresentPosition();
- Tilt.setGoalPosition(currentPosTilt+tiltInc);
+ Tilt.setGoalPosition(currentPosTilt-tiltInc);
wait_ms(500);
}
@@ -130,7 +179,7 @@
uint16_t currentPosTilt;
currentPosTilt = Tilt.getPresentPosition();
- Tilt.setGoalPosition(currentPosTilt-tiltInc);
+ Tilt.setGoalPosition(currentPosTilt+tiltInc);
wait_ms(500);
}
@@ -138,29 +187,44 @@
uint16_t currentPosPan;
currentPosPan = Pan.getPresentPosition();
+ Pan.setGoalPosition(currentPosPan-panInc);
+ wait_ms(500);
+
+ /*
+ currentPosPan = Pan.getPresentPosition();
Pan.setMovingSpeed(1124);
- Pan.setGoalPosition(4095);
+ Pan.setGoalPosition(2000); //4095
+ */
}
void panIncrementRight() {
uint16_t currentPosPan;
currentPosPan = Pan.getPresentPosition();
+ Pan.setGoalPosition(currentPosPan+panInc);
+ wait_ms(500);
+
+ /*
+ currentPosPan = Pan.getPresentPosition();
Pan.setMovingSpeed(1124);
- Pan.setGoalPosition(0);
+ Pan.setGoalPosition(500); //0
+ */
}
int main()
{
pc.baud(9600);
- //setSpeed(200);
+ setSpeed(100);
uint16_t currentPosTilt;
uint16_t currentPosPan;
Pan.setCWAngleLimit(0);
Pan.setCCWAngleLimit(4095);
- Pan.setGoalPosition(500);
- Tilt.setGoalPosition(700);
+ Pan.setGoalPosition(300);
+ Tilt.setGoalPosition(1250);
+ Base.setGoalPosition(60);
+ Elbow.setGoalPosition(360);
+ Wrist.setGoalPosition(65);
while(1) {
//if (echo == 0) {
//Base.setGoalPosition(1300);
@@ -169,12 +233,14 @@
//}
echo = pc.getc();
+
pc.printf("\r\n %d \r\n", echo);
pc.putc(echo);
- /*if (echo == 49) { retractedPosition(); }
+ if (echo == 49) { retractedPosition(); }
else if (echo == 50) { centrePosition(); }
else if (echo == 51) { leftPosition(); }
- else if (echo == 52) { rightPosition(); }*/
+ else if (echo == 52) { rightPosition(); }
+
if (echo == 119) { tiltIncrementUp(); }
else if (echo == 115) { tiltIncrementDown(); }
else if (echo == 97) { panIncrementLeft(); }
@@ -185,10 +251,6 @@
currentPosPan = Pan.getPresentPosition();
Pan.setGoalPosition(currentPosPan);
}
- echo = 0;
- echo = pc.getc();
- pc.printf("\r\n %d \r\n", echo);
- pc.putc(echo);
currentPosTilt = Tilt.getPresentPosition();