Speedy Retraction Team / Mbed 2 deprecated 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@2:05e91f425d43, 2016-07-06 (annotated)

Committer:
Mirthilon
Date:
Wed Jul 06 00:46:40 2016 +0000
Revision:
2:05e91f425d43
Parent:
1:bae7d30014b7
Child:
3:09a27cf1c9fe
Whatever you want

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daemonslayer 1:bae7d30014b7 1 /* Test MX-28 ID: 0x01
daemonslayer 1:bae7d30014b7 2 Pan RX-28 ID: 0x02 (DMXL04)
daemonslayer 1:bae7d30014b7 3 Tilt RX-28 ID: 0x03 (DMXL05)
daemonslayer 1:bae7d30014b7 4 Base MX-28 ID: 0x04
daemonslayer 1:bae7d30014b7 5 Elbow RX-28 ID: 0x05 (DMXL06)
daemonslayer 1:bae7d30014b7 6 Wrist RX-28 ID: 0x06 (DMXL08)
daemonslayer 1:bae7d30014b7 7 //DON'T GO TO ZERO ON BASE SERVO
daemonslayer 1:bae7d30014b7 8 */
daemonslayer 1:bae7d30014b7 9
daemonslayer 0:9e2ed7d4d2ea 10 #include "mbed.h"
daemonslayer 0:9e2ed7d4d2ea 11 #include "Dynamixel.h"
daemonslayer 0:9e2ed7d4d2ea 12 Serial pc(USBTX,USBRX);
daemonslayer 1:bae7d30014b7 13 Dynamixel Base(PA_9, PA_10, PB_3, 4, 57600);
daemonslayer 1:bae7d30014b7 14 Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600);
daemonslayer 1:bae7d30014b7 15 DigitalOut myled(LED1);
daemonslayer 1:bae7d30014b7 16 uint8_t pos;
Mirthilon 2:05e91f425d43 17 int echo;
daemonslayer 1:bae7d30014b7 18
daemonslayer 1:bae7d30014b7 19 void centrePosition() {
daemonslayer 1:bae7d30014b7 20 Base.setGoalPosition(800);
daemonslayer 1:bae7d30014b7 21 Elbow.setGoalPosition(50);
daemonslayer 1:bae7d30014b7 22 pos = 2;
daemonslayer 1:bae7d30014b7 23 }
daemonslayer 1:bae7d30014b7 24
daemonslayer 1:bae7d30014b7 25 void leftPosition() {
daemonslayer 1:bae7d30014b7 26 uint16_t currentPosBase;
daemonslayer 1:bae7d30014b7 27 uint16_t currentPosElbow;
daemonslayer 1:bae7d30014b7 28
daemonslayer 1:bae7d30014b7 29 if (pos == 1) {
daemonslayer 1:bae7d30014b7 30 centrePosition();
daemonslayer 1:bae7d30014b7 31 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 32 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 33 while(currentPosBase > 820 && currentPosElbow > 70) {
daemonslayer 1:bae7d30014b7 34 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 35 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 36 }
daemonslayer 1:bae7d30014b7 37 }
daemonslayer 1:bae7d30014b7 38 Base.setGoalPosition(300);
daemonslayer 1:bae7d30014b7 39 Elbow.setGoalPosition(50);
daemonslayer 1:bae7d30014b7 40 pos = 3;
daemonslayer 1:bae7d30014b7 41 }
daemonslayer 1:bae7d30014b7 42
daemonslayer 1:bae7d30014b7 43 void rightPosition() {
daemonslayer 1:bae7d30014b7 44 uint16_t currentPosBase;
daemonslayer 1:bae7d30014b7 45 uint16_t currentPosElbow;
daemonslayer 1:bae7d30014b7 46
daemonslayer 1:bae7d30014b7 47 if (pos == 1) {
daemonslayer 1:bae7d30014b7 48 centrePosition();
daemonslayer 1:bae7d30014b7 49 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 50 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 51 while(currentPosBase > 820 && currentPosElbow > 70) {
daemonslayer 1:bae7d30014b7 52 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 53 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 54 }
daemonslayer 1:bae7d30014b7 55 }
daemonslayer 1:bae7d30014b7 56 Base.setGoalPosition(1300);
daemonslayer 1:bae7d30014b7 57 Elbow.setGoalPosition(50);
daemonslayer 1:bae7d30014b7 58 pos = 4;
daemonslayer 1:bae7d30014b7 59 }
daemonslayer 1:bae7d30014b7 60
daemonslayer 1:bae7d30014b7 61 void retractedPosition() {
daemonslayer 1:bae7d30014b7 62 uint16_t currentPosBase;
daemonslayer 1:bae7d30014b7 63
daemonslayer 1:bae7d30014b7 64 if (pos == 3) {
daemonslayer 1:bae7d30014b7 65 centrePosition();
daemonslayer 1:bae7d30014b7 66 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 67 while(currentPosBase < 780) {
daemonslayer 1:bae7d30014b7 68 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 69 }
daemonslayer 1:bae7d30014b7 70 }
daemonslayer 1:bae7d30014b7 71 else if (pos == 4) {
daemonslayer 1:bae7d30014b7 72 centrePosition();
daemonslayer 1:bae7d30014b7 73 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 74 while(currentPosBase > 820) {
daemonslayer 1:bae7d30014b7 75 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 76 }
daemonslayer 1:bae7d30014b7 77 }
daemonslayer 1:bae7d30014b7 78 Base.setGoalPosition(1300);
daemonslayer 1:bae7d30014b7 79 Elbow.setGoalPosition(365);
daemonslayer 1:bae7d30014b7 80 pos = 1;
daemonslayer 1:bae7d30014b7 81 }
daemonslayer 1:bae7d30014b7 82
daemonslayer 1:bae7d30014b7 83 void measureTime() {
daemonslayer 1:bae7d30014b7 84 uint16_t currentPosBase;
daemonslayer 1:bae7d30014b7 85 Timer t;
daemonslayer 1:bae7d30014b7 86 uint32_t time;
daemonslayer 1:bae7d30014b7 87
daemonslayer 1:bae7d30014b7 88 t.start();
daemonslayer 1:bae7d30014b7 89 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 90 while(currentPosBase < 1280) {
daemonslayer 1:bae7d30014b7 91 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 92 }
daemonslayer 1:bae7d30014b7 93 time = t.read_ms();
daemonslayer 1:bae7d30014b7 94 pc.printf("Time: %d\r\n",time);
daemonslayer 1:bae7d30014b7 95 t.stop();
daemonslayer 1:bae7d30014b7 96 t.reset();
daemonslayer 1:bae7d30014b7 97 }
daemonslayer 1:bae7d30014b7 98
daemonslayer 1:bae7d30014b7 99 void setSpeed(uint16_t speed) {
daemonslayer 1:bae7d30014b7 100 Base.setMovingSpeed(1024+speed);
daemonslayer 1:bae7d30014b7 101 Elbow.setMovingSpeed(1024+2.2*speed);
daemonslayer 1:bae7d30014b7 102 }
daemonslayer 1:bae7d30014b7 103
daemonslayer 1:bae7d30014b7 104 void setLimits() {
daemonslayer 1:bae7d30014b7 105 Base.setCWAngleLimit(300);
daemonslayer 1:bae7d30014b7 106 Elbow.setCWAngleLimit(50);
daemonslayer 1:bae7d30014b7 107 Base.setCCWAngleLimit(1300);
daemonslayer 1:bae7d30014b7 108 Elbow.setCCWAngleLimit(365);
daemonslayer 1:bae7d30014b7 109 }
daemonslayer 0:9e2ed7d4d2ea 110
daemonslayer 0:9e2ed7d4d2ea 111 int main()
daemonslayer 0:9e2ed7d4d2ea 112 {
daemonslayer 0:9e2ed7d4d2ea 113 pc.baud(9600);
daemonslayer 1:bae7d30014b7 114 setSpeed(200);
daemonslayer 0:9e2ed7d4d2ea 115
daemonslayer 1:bae7d30014b7 116 while(1) {
Mirthilon 2:05e91f425d43 117 if (echo == 0) {
Mirthilon 2:05e91f425d43 118 Base.setGoalPosition(1300);
Mirthilon 2:05e91f425d43 119 Elbow.setGoalPosition(365);
Mirthilon 2:05e91f425d43 120 }
Mirthilon 2:05e91f425d43 121
Mirthilon 2:05e91f425d43 122 echo = pc.getc();
Mirthilon 2:05e91f425d43 123 pc.printf("\r\n");
Mirthilon 2:05e91f425d43 124 pc.putc(echo);
Mirthilon 2:05e91f425d43 125 if (echo == 49) { retractedPosition(); }
Mirthilon 2:05e91f425d43 126 else if (echo == 50) { centrePosition(); }
Mirthilon 2:05e91f425d43 127 else if (echo == 51) { leftPosition(); }
Mirthilon 2:05e91f425d43 128 else if (echo == 52) { rightPosition(); }
Mirthilon 2:05e91f425d43 129 else { continue; }
daemonslayer 1:bae7d30014b7 130 wait(2);
Mirthilon 2:05e91f425d43 131
Mirthilon 2:05e91f425d43 132 /*
daemonslayer 1:bae7d30014b7 133 retractedPosition();
Mirthilon 2:05e91f425d43 134 wait(3);
Mirthilon 2:05e91f425d43 135 leftPosition();
Mirthilon 2:05e91f425d43 136 wait(3);
daemonslayer 1:bae7d30014b7 137 rightPosition();
Mirthilon 2:05e91f425d43 138 wait(3);
daemonslayer 1:bae7d30014b7 139 centrePosition();
Mirthilon 2:05e91f425d43 140 wait(3);
Mirthilon 2:05e91f425d43 141 retractedPosition();
Mirthilon 2:05e91f425d43 142 wait(3);
Mirthilon 2:05e91f425d43 143 rightPosition();
Mirthilon 2:05e91f425d43 144 wait(3);
Mirthilon 2:05e91f425d43 145 centrePosition();
Mirthilon 2:05e91f425d43 146 wait(3);
daemonslayer 1:bae7d30014b7 147 leftPosition();
Mirthilon 2:05e91f425d43 148 wait(3);*/
daemonslayer 1:bae7d30014b7 149 }
Mirthilon 2:05e91f425d43 150
daemonslayer 1:bae7d30014b7 151
daemonslayer 1:bae7d30014b7 152 }
daemonslayer 1:bae7d30014b7 153
daemonslayer 1:bae7d30014b7 154 /*//MX28a.reset();
daemonslayer 1:bae7d30014b7 155 MX28a.setId(6);
daemonslayer 1:bae7d30014b7 156 //wait(1);
daemonslayer 1:bae7d30014b7 157 MX28a.setEnableLED(1);
daemonslayer 1:bae7d30014b7 158 //RX28a.setEnableLED(0);
daemonslayer 1:bae7d30014b7 159 wait(1);
daemonslayer 1:bae7d30014b7 160 MX28a.setEnableLED(0);
daemonslayer 1:bae7d30014b7 161 //RX28a.setEnableLED(1);
daemonslayer 1:bae7d30014b7 162 wait(1);
daemonslayer 1:bae7d30014b7 163 MX28a.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 164 RX28a.setGoalPosition(1023);
daemonslayer 1:bae7d30014b7 165 wait(1);
daemonslayer 1:bae7d30014b7 166 MX28a.setGoalPosition(4095);
daemonslayer 1:bae7d30014b7 167 RX28a.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 168 wait(1);
daemonslayer 1:bae7d30014b7 169 delayTest = MX28a.getReturnDelayTime();
daemonslayer 1:bae7d30014b7 170 pc.printf("Delay Time: 0x%04x\r\n",delayTest);
daemonslayer 1:bae7d30014b7 171 wait(1);
daemonslayer 1:bae7d30014b7 172 modelNumber = MX28a.getModelNumber();
daemonslayer 1:bae7d30014b7 173 pc.printf("Model Number: 0x%04x\r\n",modelNumber);
daemonslayer 1:bae7d30014b7 174 wait(1);
daemonslayer 1:bae7d30014b7 175 id = MX28a.getId();
daemonslayer 1:bae7d30014b7 176 pc.printf("ID Number: 0x%04x\r\n",id);
daemonslayer 1:bae7d30014b7 177 if (id == 0x01) {
daemonslayer 1:bae7d30014b7 178 myled = !myled;
daemonslayer 1:bae7d30014b7 179 }
daemonslayer 1:bae7d30014b7 180 wait(1);
daemonslayer 1:bae7d30014b7 181 baud = MX28a.getBaudRate();
daemonslayer 1:bae7d30014b7 182 pc.printf("Baud Rate: %d\r\n",baud);
daemonslayer 0:9e2ed7d4d2ea 183
daemonslayer 1:bae7d30014b7 184
daemonslayer 1:bae7d30014b7 185 */
daemonslayer 1:bae7d30014b7 186 /*int rar = 1;
daemonslayer 1:bae7d30014b7 187 int goalPos = 60;
daemonslayer 1:bae7d30014b7 188
daemonslayer 1:bae7d30014b7 189 int Xm = 0;
daemonslayer 1:bae7d30014b7 190 int Ym = 0;
daemonslayer 1:bae7d30014b7 191 int Xe = 0;
daemonslayer 1:bae7d30014b7 192 int Ye = 0;
daemonslayer 1:bae7d30014b7 193
daemonslayer 1:bae7d30014b7 194 int L = 147; // mm
daemonslayer 1:bae7d30014b7 195 int S = 97; // mm
daemonslayer 1:bae7d30014b7 196 int thet = 0;
daemonslayer 1:bae7d30014b7 197 int gamm = 0;*/
daemonslayer 1:bae7d30014b7 198
daemonslayer 1:bae7d30014b7 199 /*currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 200 pc.printf("Base Current Position: %d\r\n",currentPosBase);
daemonslayer 1:bae7d30014b7 201 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 202 pc.printf("Elbow Current Position: %d\r\n",currentPosElbow);
daemonslayer 1:bae7d30014b7 203 //wait(3);
daemonslayer 1:bae7d30014b7 204 if (currentPosElbow == 360 && rar == 1) {
daemonslayer 1:bae7d30014b7 205 rar = -1;
daemonslayer 1:bae7d30014b7 206 }
daemonslayer 1:bae7d30014b7 207 else if (currentPosElbow == 60 && rar == -1) {
daemonslayer 1:bae7d30014b7 208 rar = 1;
daemonslayer 1:bae7d30014b7 209 }
daemonslayer 1:bae7d30014b7 210 Elbow.setGoalPosition(goalPos);
daemonslayer 1:bae7d30014b7 211
daemonslayer 1:bae7d30014b7 212 goalPos += rar;*/
daemonslayer 1:bae7d30014b7 213 //error = Base.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 214 //Elbow.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 215 //pc.printf("Error: 0x%04x\r\n",error);
daemonslayer 1:bae7d30014b7 216 //wait(1);