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@1:bae7d30014b7, 2016-07-05 (annotated)

Committer:
daemonslayer
Date:
Tue Jul 05 12:11:26 2016 +0000
Revision:
1:bae7d30014b7
Parent:
0:9e2ed7d4d2ea
Child:
2:05e91f425d43
Moves to all 4 positions

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