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@5:c74f23e4325f, 2016-07-10 (annotated)

Committer:
Mirthilon
Date:
Sun Jul 10 04:23:11 2016 +0000
Revision:
5:c74f23e4325f
Parent:
4:0c1555b01361
Child:
6:414eb29191ad
Shorter wait time

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mirthilon 4:0c1555b01361 1 /* Tilt MX-28 ID: 0x01
Mirthilon 4:0c1555b01361 2 Test RX-28 ID: 0x02 (DMXL04)
Mirthilon 4:0c1555b01361 3 Wrist RX-28 ID: 0x03 (DMXL05)
Mirthilon 4:0c1555b01361 4 Pan MX-28 ID: 0x04
daemonslayer 1:bae7d30014b7 5 Elbow RX-28 ID: 0x05 (DMXL06)
Mirthilon 4:0c1555b01361 6 Base 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);
Mirthilon 4:0c1555b01361 13 Dynamixel Base(PA_9, PA_10, PB_3, 6, 57600); // RX-28
Mirthilon 4:0c1555b01361 14 Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600); // RX-28
Mirthilon 4:0c1555b01361 15 Dynamixel Wrist(PA_9, PA_10, PB_3, 2, 57600); // RX-28
Mirthilon 4:0c1555b01361 16 Dynamixel Pan(PA_9, PA_10, PB_3, 4, 57600); // MX-28
Mirthilon 4:0c1555b01361 17 Dynamixel Tilt(PA_9, PA_10, PB_3, 1, 57600); // MX-28
Mirthilon 4:0c1555b01361 18
daemonslayer 1:bae7d30014b7 19 DigitalOut myled(LED1);
Mirthilon 4:0c1555b01361 20 uint8_t pos = 1;
Mirthilon 5:c74f23e4325f 21 uint16_t tiltInc = 50; // was 15 for RX
Mirthilon 5:c74f23e4325f 22 uint16_t panInc = 50;
Mirthilon 2:05e91f425d43 23 int echo;
daemonslayer 1:bae7d30014b7 24
daemonslayer 1:bae7d30014b7 25 void centrePosition() {
Mirthilon 4:0c1555b01361 26 if (pos == 1) {
Mirthilon 4:0c1555b01361 27 Wrist.setMovingSpeed(1024+1.4*100); // SPEEEEEEEED
Mirthilon 4:0c1555b01361 28 Base.setMovingSpeed(1024+100); //Speeed
Mirthilon 4:0c1555b01361 29 Elbow.setMovingSpeed(1024+2.3*100); //Speeed
Mirthilon 4:0c1555b01361 30 }
Mirthilon 4:0c1555b01361 31 else if (pos == 3) {
Mirthilon 4:0c1555b01361 32 Wrist.setMovingSpeed(1024+150); // SPEEEEEEEED
Mirthilon 4:0c1555b01361 33 Base.setMovingSpeed(1024+50); //Speeed
Mirthilon 4:0c1555b01361 34 Elbow.setMovingSpeed(1024+100); //Speeed
Mirthilon 4:0c1555b01361 35 }
Mirthilon 4:0c1555b01361 36 else if (pos == 4) {
Mirthilon 4:0c1555b01361 37 Wrist.setMovingSpeed(1024+70); // SPEEEEEEEED
Mirthilon 4:0c1555b01361 38 Base.setMovingSpeed(1024+100); //Speeed
Mirthilon 4:0c1555b01361 39 Elbow.setMovingSpeed(1024+50); //Speeed
Mirthilon 4:0c1555b01361 40 }
Mirthilon 4:0c1555b01361 41
Mirthilon 4:0c1555b01361 42
Mirthilon 4:0c1555b01361 43 Base.setGoalPosition(150);
Mirthilon 4:0c1555b01361 44 Elbow.setGoalPosition(150);
Mirthilon 5:c74f23e4325f 45 Wrist.setGoalPosition(180);
daemonslayer 1:bae7d30014b7 46 pos = 2;
Mirthilon 3:09a27cf1c9fe 47 wait(2);
daemonslayer 1:bae7d30014b7 48 }
daemonslayer 1:bae7d30014b7 49
daemonslayer 1:bae7d30014b7 50 void leftPosition() {
daemonslayer 1:bae7d30014b7 51 uint16_t currentPosBase;
daemonslayer 1:bae7d30014b7 52 uint16_t currentPosElbow;
Mirthilon 4:0c1555b01361 53 Wrist.setMovingSpeed(1024+1.5*100); // SPEEEEEEEED
daemonslayer 1:bae7d30014b7 54
daemonslayer 1:bae7d30014b7 55 if (pos == 1) {
daemonslayer 1:bae7d30014b7 56 centrePosition();
daemonslayer 1:bae7d30014b7 57 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 58 currentPosElbow = Elbow.getPresentPosition();
Mirthilon 5:c74f23e4325f 59 while(currentPosBase < 140 && currentPosElbow > 160) {
daemonslayer 1:bae7d30014b7 60 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 61 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 62 }
daemonslayer 1:bae7d30014b7 63 }
Mirthilon 5:c74f23e4325f 64 Wrist.setMovingSpeed(1024+80);
Mirthilon 5:c74f23e4325f 65 if (pos == 4) {
Mirthilon 5:c74f23e4325f 66 Wrist.setMovingSpeed(1024+50);
Mirthilon 5:c74f23e4325f 67 }
Mirthilon 5:c74f23e4325f 68 // SPEEEEEEEED
Mirthilon 4:0c1555b01361 69 Elbow.setMovingSpeed(1024+50); //Speeed
Mirthilon 4:0c1555b01361 70 Base.setMovingSpeed(1024+30); //Speeed
Mirthilon 4:0c1555b01361 71
Mirthilon 4:0c1555b01361 72 Base.setGoalPosition(70);
Mirthilon 4:0c1555b01361 73 Elbow.setGoalPosition(60);
Mirthilon 5:c74f23e4325f 74 Wrist.setGoalPosition(370);
daemonslayer 1:bae7d30014b7 75 pos = 3;
Mirthilon 3:09a27cf1c9fe 76 wait(2);
daemonslayer 1:bae7d30014b7 77 }
daemonslayer 1:bae7d30014b7 78
daemonslayer 1:bae7d30014b7 79 void rightPosition() {
daemonslayer 1:bae7d30014b7 80 uint16_t currentPosBase;
daemonslayer 1:bae7d30014b7 81 uint16_t currentPosElbow;
Mirthilon 4:0c1555b01361 82 Wrist.setMovingSpeed(1024+1.5*100); // SPEEEEEEEED
daemonslayer 1:bae7d30014b7 83
daemonslayer 1:bae7d30014b7 84 if (pos == 1) {
daemonslayer 1:bae7d30014b7 85 centrePosition();
daemonslayer 1:bae7d30014b7 86 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 87 currentPosElbow = Elbow.getPresentPosition();
Mirthilon 4:0c1555b01361 88 while(currentPosBase < 145 && currentPosElbow > 155) {
daemonslayer 1:bae7d30014b7 89 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 90 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 91 }
daemonslayer 1:bae7d30014b7 92 }
Mirthilon 4:0c1555b01361 93 if (pos == 3) {
Mirthilon 4:0c1555b01361 94 Base.setGoalPosition(150); // To lessen lever disadvantage
Mirthilon 5:c74f23e4325f 95 Elbow.setGoalPosition(150);
Mirthilon 5:c74f23e4325f 96 Wrist.setMovingSpeed(1024+50);
Mirthilon 5:c74f23e4325f 97 Wrist.setGoalPosition(140);
Mirthilon 5:c74f23e4325f 98 wait(1);
Mirthilon 4:0c1555b01361 99 }
Mirthilon 4:0c1555b01361 100
Mirthilon 5:c74f23e4325f 101 Wrist.setMovingSpeed(1024+110); // SPEEEEEEEED
Mirthilon 4:0c1555b01361 102 Elbow.setMovingSpeed(1024+50); //Speeed
Mirthilon 4:0c1555b01361 103 Base.setMovingSpeed(1024+100); //Speeed
Mirthilon 4:0c1555b01361 104 Base.setGoalPosition(350);
daemonslayer 1:bae7d30014b7 105 Elbow.setGoalPosition(50);
Mirthilon 5:c74f23e4325f 106 wait_us(100);
Mirthilon 4:0c1555b01361 107 Wrist.setGoalPosition(60);
daemonslayer 1:bae7d30014b7 108 pos = 4;
Mirthilon 3:09a27cf1c9fe 109 wait(2);
daemonslayer 1:bae7d30014b7 110 }
daemonslayer 1:bae7d30014b7 111
daemonslayer 1:bae7d30014b7 112 void retractedPosition() {
daemonslayer 1:bae7d30014b7 113 uint16_t currentPosBase;
Mirthilon 4:0c1555b01361 114 uint16_t currentPosElbow;
Mirthilon 4:0c1555b01361 115 Wrist.setMovingSpeed(1024+100); // SPEEEEEEEED
Mirthilon 5:c74f23e4325f 116 Base.setMovingSpeed(1024+100);
daemonslayer 1:bae7d30014b7 117 if (pos == 3) {
daemonslayer 1:bae7d30014b7 118 centrePosition();
daemonslayer 1:bae7d30014b7 119 currentPosBase = Base.getPresentPosition();
Mirthilon 4:0c1555b01361 120 currentPosElbow = Elbow.getPresentPosition();
Mirthilon 4:0c1555b01361 121 while(currentPosBase < 155 && currentPosElbow < 145) {
daemonslayer 1:bae7d30014b7 122 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 123 }
Mirthilon 4:0c1555b01361 124 Base.setMovingSpeed(1024+120);
daemonslayer 1:bae7d30014b7 125 }
daemonslayer 1:bae7d30014b7 126 else if (pos == 4) {
daemonslayer 1:bae7d30014b7 127 centrePosition();
daemonslayer 1:bae7d30014b7 128 currentPosBase = Base.getPresentPosition();
Mirthilon 4:0c1555b01361 129 currentPosElbow = Elbow.getPresentPosition();
Mirthilon 4:0c1555b01361 130 while(currentPosBase > 155 && currentPosElbow < 145) {
daemonslayer 1:bae7d30014b7 131 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 132 }
daemonslayer 1:bae7d30014b7 133 }
Mirthilon 4:0c1555b01361 134 Wrist.setMovingSpeed(1024+1.4*100); // SPEEEEEEEED
Mirthilon 4:0c1555b01361 135 Elbow.setMovingSpeed(1024+2.3*100); //Speeed
Mirthilon 4:0c1555b01361 136 //if (pos == 3) {
Mirthilon 4:0c1555b01361 137
Mirthilon 4:0c1555b01361 138 //}
Mirthilon 4:0c1555b01361 139
Mirthilon 4:0c1555b01361 140 Base.setGoalPosition(60);
Mirthilon 5:c74f23e4325f 141 Elbow.setGoalPosition(350);
Mirthilon 5:c74f23e4325f 142 Wrist.setGoalPosition(60);
daemonslayer 1:bae7d30014b7 143 pos = 1;
Mirthilon 3:09a27cf1c9fe 144 wait(2);
daemonslayer 1:bae7d30014b7 145 }
daemonslayer 1:bae7d30014b7 146
daemonslayer 1:bae7d30014b7 147 void measureTime() {
daemonslayer 1:bae7d30014b7 148 uint16_t currentPosBase;
daemonslayer 1:bae7d30014b7 149 Timer t;
daemonslayer 1:bae7d30014b7 150 uint32_t time;
daemonslayer 1:bae7d30014b7 151
daemonslayer 1:bae7d30014b7 152 t.start();
daemonslayer 1:bae7d30014b7 153 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 154 while(currentPosBase < 1280) {
daemonslayer 1:bae7d30014b7 155 currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 156 }
daemonslayer 1:bae7d30014b7 157 time = t.read_ms();
daemonslayer 1:bae7d30014b7 158 pc.printf("Time: %d\r\n",time);
daemonslayer 1:bae7d30014b7 159 t.stop();
daemonslayer 1:bae7d30014b7 160 t.reset();
daemonslayer 1:bae7d30014b7 161 }
daemonslayer 1:bae7d30014b7 162
daemonslayer 1:bae7d30014b7 163 void setSpeed(uint16_t speed) {
daemonslayer 1:bae7d30014b7 164 Base.setMovingSpeed(1024+speed);
Mirthilon 4:0c1555b01361 165 Elbow.setMovingSpeed(1024+2.3*speed); // was 1024+2.2*speed
Mirthilon 4:0c1555b01361 166 Wrist.setMovingSpeed(1024+1.4*speed);
Mirthilon 5:c74f23e4325f 167 Pan.setMovingSpeed(1024+0.3*speed);
Mirthilon 5:c74f23e4325f 168 Tilt.setMovingSpeed(1024+0.3*speed);
daemonslayer 1:bae7d30014b7 169 }
daemonslayer 1:bae7d30014b7 170
daemonslayer 1:bae7d30014b7 171 void setLimits() {
daemonslayer 1:bae7d30014b7 172 Base.setCWAngleLimit(300);
daemonslayer 1:bae7d30014b7 173 Elbow.setCWAngleLimit(50);
daemonslayer 1:bae7d30014b7 174 Base.setCCWAngleLimit(1300);
daemonslayer 1:bae7d30014b7 175 Elbow.setCCWAngleLimit(365);
Mirthilon 3:09a27cf1c9fe 176 Tilt.setCWAngleLimit(210);
Mirthilon 3:09a27cf1c9fe 177 Tilt.setCCWAngleLimit(1000);
Mirthilon 3:09a27cf1c9fe 178 }
Mirthilon 3:09a27cf1c9fe 179
Mirthilon 3:09a27cf1c9fe 180 void tiltIncrementUp() {
Mirthilon 3:09a27cf1c9fe 181 uint16_t currentPosTilt;
Mirthilon 3:09a27cf1c9fe 182
Mirthilon 3:09a27cf1c9fe 183 currentPosTilt = Tilt.getPresentPosition();
Mirthilon 4:0c1555b01361 184 Tilt.setGoalPosition(currentPosTilt-tiltInc);
Mirthilon 5:c74f23e4325f 185 wait_ms(10);
Mirthilon 3:09a27cf1c9fe 186 }
Mirthilon 3:09a27cf1c9fe 187
Mirthilon 3:09a27cf1c9fe 188 void tiltIncrementDown() {
Mirthilon 3:09a27cf1c9fe 189 uint16_t currentPosTilt;
Mirthilon 3:09a27cf1c9fe 190
Mirthilon 3:09a27cf1c9fe 191 currentPosTilt = Tilt.getPresentPosition();
Mirthilon 4:0c1555b01361 192 Tilt.setGoalPosition(currentPosTilt+tiltInc);
Mirthilon 5:c74f23e4325f 193 wait_ms(10);
Mirthilon 3:09a27cf1c9fe 194 }
Mirthilon 3:09a27cf1c9fe 195
Mirthilon 3:09a27cf1c9fe 196 void panIncrementLeft() {
Mirthilon 3:09a27cf1c9fe 197 uint16_t currentPosPan;
Mirthilon 3:09a27cf1c9fe 198
Mirthilon 3:09a27cf1c9fe 199 currentPosPan = Pan.getPresentPosition();
Mirthilon 5:c74f23e4325f 200 Pan.setGoalPosition(currentPosPan+panInc);
Mirthilon 5:c74f23e4325f 201 wait_ms(10);
Mirthilon 4:0c1555b01361 202
Mirthilon 4:0c1555b01361 203 /*
Mirthilon 4:0c1555b01361 204 currentPosPan = Pan.getPresentPosition();
Mirthilon 3:09a27cf1c9fe 205 Pan.setMovingSpeed(1124);
Mirthilon 4:0c1555b01361 206 Pan.setGoalPosition(2000); //4095
Mirthilon 4:0c1555b01361 207 */
Mirthilon 3:09a27cf1c9fe 208 }
Mirthilon 3:09a27cf1c9fe 209
Mirthilon 3:09a27cf1c9fe 210 void panIncrementRight() {
Mirthilon 3:09a27cf1c9fe 211 uint16_t currentPosPan;
Mirthilon 3:09a27cf1c9fe 212
Mirthilon 3:09a27cf1c9fe 213 currentPosPan = Pan.getPresentPosition();
Mirthilon 5:c74f23e4325f 214 Pan.setGoalPosition(currentPosPan-panInc);
Mirthilon 5:c74f23e4325f 215 wait_ms(10);
Mirthilon 4:0c1555b01361 216
Mirthilon 4:0c1555b01361 217 /*
Mirthilon 4:0c1555b01361 218 currentPosPan = Pan.getPresentPosition();
Mirthilon 3:09a27cf1c9fe 219 Pan.setMovingSpeed(1124);
Mirthilon 4:0c1555b01361 220 Pan.setGoalPosition(500); //0
Mirthilon 4:0c1555b01361 221 */
daemonslayer 1:bae7d30014b7 222 }
daemonslayer 0:9e2ed7d4d2ea 223
daemonslayer 0:9e2ed7d4d2ea 224 int main()
daemonslayer 0:9e2ed7d4d2ea 225 {
daemonslayer 0:9e2ed7d4d2ea 226 pc.baud(9600);
Mirthilon 4:0c1555b01361 227 setSpeed(100);
Mirthilon 3:09a27cf1c9fe 228 uint16_t currentPosTilt;
Mirthilon 3:09a27cf1c9fe 229 uint16_t currentPosPan;
Mirthilon 3:09a27cf1c9fe 230 Pan.setCWAngleLimit(0);
Mirthilon 3:09a27cf1c9fe 231 Pan.setCCWAngleLimit(4095);
Mirthilon 5:c74f23e4325f 232 Pan.setGoalPosition(1850);
Mirthilon 5:c74f23e4325f 233 Tilt.setGoalPosition(195);
daemonslayer 0:9e2ed7d4d2ea 234
Mirthilon 4:0c1555b01361 235 Base.setGoalPosition(60);
Mirthilon 5:c74f23e4325f 236 Elbow.setGoalPosition(350);
Mirthilon 5:c74f23e4325f 237 Wrist.setGoalPosition(50);
daemonslayer 1:bae7d30014b7 238 while(1) {
Mirthilon 3:09a27cf1c9fe 239 //if (echo == 0) {
Mirthilon 3:09a27cf1c9fe 240 //Base.setGoalPosition(1300);
Mirthilon 3:09a27cf1c9fe 241 //Elbow.setGoalPosition(365);
Mirthilon 3:09a27cf1c9fe 242
Mirthilon 3:09a27cf1c9fe 243 //}
Mirthilon 2:05e91f425d43 244
Mirthilon 2:05e91f425d43 245 echo = pc.getc();
Mirthilon 4:0c1555b01361 246
Mirthilon 5:c74f23e4325f 247 //pc.printf("\r\n %d \r\n", echo);
Mirthilon 2:05e91f425d43 248 pc.putc(echo);
Mirthilon 4:0c1555b01361 249 if (echo == 49) { retractedPosition(); }
Mirthilon 2:05e91f425d43 250 else if (echo == 50) { centrePosition(); }
Mirthilon 2:05e91f425d43 251 else if (echo == 51) { leftPosition(); }
Mirthilon 4:0c1555b01361 252 else if (echo == 52) { rightPosition(); }
Mirthilon 4:0c1555b01361 253
Mirthilon 3:09a27cf1c9fe 254 if (echo == 119) { tiltIncrementUp(); }
Mirthilon 3:09a27cf1c9fe 255 else if (echo == 115) { tiltIncrementDown(); }
Mirthilon 3:09a27cf1c9fe 256 else if (echo == 97) { panIncrementLeft(); }
Mirthilon 3:09a27cf1c9fe 257 else if (echo == 100) { panIncrementRight(); }
Mirthilon 3:09a27cf1c9fe 258 else {
Mirthilon 5:c74f23e4325f 259 //Tilt.setMovingSpeed(1024);
Mirthilon 5:c74f23e4325f 260 //Pan.setMovingSpeed(1024);
Mirthilon 3:09a27cf1c9fe 261 currentPosPan = Pan.getPresentPosition();
Mirthilon 3:09a27cf1c9fe 262 Pan.setGoalPosition(currentPosPan);
Mirthilon 3:09a27cf1c9fe 263 }
Mirthilon 3:09a27cf1c9fe 264
Mirthilon 3:09a27cf1c9fe 265 currentPosTilt = Tilt.getPresentPosition();
Mirthilon 5:c74f23e4325f 266 pc.printf("Tilt Position: %d\r\n",currentPosTilt);
Mirthilon 3:09a27cf1c9fe 267 currentPosPan = Pan.getPresentPosition();
Mirthilon 5:c74f23e4325f 268 pc.printf("Pan Position: %d\r\n",currentPosPan);
Mirthilon 3:09a27cf1c9fe 269
Mirthilon 2:05e91f425d43 270
Mirthilon 2:05e91f425d43 271 /*
daemonslayer 1:bae7d30014b7 272 retractedPosition();
Mirthilon 2:05e91f425d43 273 wait(3);
Mirthilon 2:05e91f425d43 274 leftPosition();
Mirthilon 2:05e91f425d43 275 wait(3);
daemonslayer 1:bae7d30014b7 276 rightPosition();
Mirthilon 2:05e91f425d43 277 wait(3);
daemonslayer 1:bae7d30014b7 278 centrePosition();
Mirthilon 2:05e91f425d43 279 wait(3);
Mirthilon 2:05e91f425d43 280 retractedPosition();
Mirthilon 2:05e91f425d43 281 wait(3);
Mirthilon 2:05e91f425d43 282 rightPosition();
Mirthilon 2:05e91f425d43 283 wait(3);
Mirthilon 2:05e91f425d43 284 centrePosition();
Mirthilon 2:05e91f425d43 285 wait(3);
daemonslayer 1:bae7d30014b7 286 leftPosition();
Mirthilon 2:05e91f425d43 287 wait(3);*/
daemonslayer 1:bae7d30014b7 288 }
Mirthilon 2:05e91f425d43 289
daemonslayer 1:bae7d30014b7 290
daemonslayer 1:bae7d30014b7 291 }
daemonslayer 1:bae7d30014b7 292
daemonslayer 1:bae7d30014b7 293 /*//MX28a.reset();
daemonslayer 1:bae7d30014b7 294 MX28a.setId(6);
daemonslayer 1:bae7d30014b7 295 //wait(1);
daemonslayer 1:bae7d30014b7 296 MX28a.setEnableLED(1);
daemonslayer 1:bae7d30014b7 297 //RX28a.setEnableLED(0);
daemonslayer 1:bae7d30014b7 298 wait(1);
daemonslayer 1:bae7d30014b7 299 MX28a.setEnableLED(0);
daemonslayer 1:bae7d30014b7 300 //RX28a.setEnableLED(1);
daemonslayer 1:bae7d30014b7 301 wait(1);
daemonslayer 1:bae7d30014b7 302 MX28a.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 303 RX28a.setGoalPosition(1023);
daemonslayer 1:bae7d30014b7 304 wait(1);
daemonslayer 1:bae7d30014b7 305 MX28a.setGoalPosition(4095);
daemonslayer 1:bae7d30014b7 306 RX28a.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 307 wait(1);
daemonslayer 1:bae7d30014b7 308 delayTest = MX28a.getReturnDelayTime();
daemonslayer 1:bae7d30014b7 309 pc.printf("Delay Time: 0x%04x\r\n",delayTest);
daemonslayer 1:bae7d30014b7 310 wait(1);
daemonslayer 1:bae7d30014b7 311 modelNumber = MX28a.getModelNumber();
daemonslayer 1:bae7d30014b7 312 pc.printf("Model Number: 0x%04x\r\n",modelNumber);
daemonslayer 1:bae7d30014b7 313 wait(1);
daemonslayer 1:bae7d30014b7 314 id = MX28a.getId();
daemonslayer 1:bae7d30014b7 315 pc.printf("ID Number: 0x%04x\r\n",id);
daemonslayer 1:bae7d30014b7 316 if (id == 0x01) {
daemonslayer 1:bae7d30014b7 317 myled = !myled;
daemonslayer 1:bae7d30014b7 318 }
daemonslayer 1:bae7d30014b7 319 wait(1);
daemonslayer 1:bae7d30014b7 320 baud = MX28a.getBaudRate();
daemonslayer 1:bae7d30014b7 321 pc.printf("Baud Rate: %d\r\n",baud);
daemonslayer 0:9e2ed7d4d2ea 322
daemonslayer 1:bae7d30014b7 323
daemonslayer 1:bae7d30014b7 324 */
daemonslayer 1:bae7d30014b7 325 /*int rar = 1;
daemonslayer 1:bae7d30014b7 326 int goalPos = 60;
daemonslayer 1:bae7d30014b7 327
daemonslayer 1:bae7d30014b7 328 int Xm = 0;
daemonslayer 1:bae7d30014b7 329 int Ym = 0;
daemonslayer 1:bae7d30014b7 330 int Xe = 0;
daemonslayer 1:bae7d30014b7 331 int Ye = 0;
daemonslayer 1:bae7d30014b7 332
daemonslayer 1:bae7d30014b7 333 int L = 147; // mm
daemonslayer 1:bae7d30014b7 334 int S = 97; // mm
daemonslayer 1:bae7d30014b7 335 int thet = 0;
daemonslayer 1:bae7d30014b7 336 int gamm = 0;*/
daemonslayer 1:bae7d30014b7 337
daemonslayer 1:bae7d30014b7 338 /*currentPosBase = Base.getPresentPosition();
daemonslayer 1:bae7d30014b7 339 pc.printf("Base Current Position: %d\r\n",currentPosBase);
daemonslayer 1:bae7d30014b7 340 currentPosElbow = Elbow.getPresentPosition();
daemonslayer 1:bae7d30014b7 341 pc.printf("Elbow Current Position: %d\r\n",currentPosElbow);
daemonslayer 1:bae7d30014b7 342 //wait(3);
daemonslayer 1:bae7d30014b7 343 if (currentPosElbow == 360 && rar == 1) {
daemonslayer 1:bae7d30014b7 344 rar = -1;
daemonslayer 1:bae7d30014b7 345 }
daemonslayer 1:bae7d30014b7 346 else if (currentPosElbow == 60 && rar == -1) {
daemonslayer 1:bae7d30014b7 347 rar = 1;
daemonslayer 1:bae7d30014b7 348 }
daemonslayer 1:bae7d30014b7 349 Elbow.setGoalPosition(goalPos);
daemonslayer 1:bae7d30014b7 350
daemonslayer 1:bae7d30014b7 351 goalPos += rar;*/
daemonslayer 1:bae7d30014b7 352 //error = Base.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 353 //Elbow.setGoalPosition(0);
daemonslayer 1:bae7d30014b7 354 //pc.printf("Error: 0x%04x\r\n",error);
daemonslayer 1:bae7d30014b7 355 //wait(1);