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@4:0c1555b01361, 2016-07-08 (annotated)

Committer:
Mirthilon
Date:
Fri Jul 08 08:54:39 2016 +0000
Revision:
4:0c1555b01361
Parent:
3:09a27cf1c9fe
Child:
5:c74f23e4325f
Initial code to have the whole arm moving. Horribly done, sorry. Pan/tilt not altered or improved. Currently need to change the servo speeds in each transition due to the base servo barely coping with the load.

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